       integralL = integral_max;
     if(integralL < integral_min)
       integralL = integral_min;
-    cmd = feedforward(WL_ref) + error*Kp_L + integralL*Ki_L; // controller
-    Serial.println(cmd);
+    float der_error = (error - prev_error_L)/timer_dt;  
+    prev_error_L = error;
+    cmd = feedforward(WL_ref) + error*Kp_L + integralL*Ki_L + der_error*Kd_L; // controller
+    
     if(cmd>=0.0)
     {
       analogWrite(out1_pin, (int)cmd);

  int pin2_counter = decoder_pin_2_counter;
  
  // average filter of encoder data
  float average_counter_pin1 = 0.0;
  for(int i=0; i<filter_size-1; i++)
  {
    average_filter_pin1[i] = average_filter_pin1[i+1];
    average_counter_pin1 = average_counter_pin1 + average_filter_pin1[i+1];
  }  
  average_filter_pin1[filter_size-1] = pin1_counter;
  average_counter_pin1 = average_counter_pin1 + pin1_counter;
  average_counter_pin1 = average_counter_pin1/(float)filter_size;

  float average_counter_pin2 = 0.0;
  for(int i=0; i<filter_size-1; i++)
  {
    average_filter_pin2[i] = average_filter_pin2[i+1];
    average_counter_pin2 = average_counter_pin2 + average_filter_pin2[i+1];
  }  
  average_filter_pin2[filter_size-1] = pin2_counter;
  average_counter_pin2 = average_counter_pin2 + pin2_counter;
  average_counter_pin2 = average_counter_pin2/(float)filter_size;
  
  // controller (feedforward + feedback)
  float cmd = 0.0;
  float error = 0.0;

    analogWrite(out3_pin, 0);
    analogWrite(out4_pin, 0);
  }

  
  
  
  // This is a workaround to prevent from motor idle when speed  = 0;
