       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);

volatile int decoder_pin_1_delay_counter = 0;
volatile int decoder_pin_2_delay_counter = 0;
volatile int encoder_res = 18; // 20 pulse in a circle --> 360/20 = 18(deg/pulse)
volatile int average_filter_pin1[filter_size] = {}; // all zeros
volatile int average_filter_pin2[filter_size] = {}; // all zeros

// controller
volatile float WL_ref = 0.0; //reference speed for left wheel (deg/sec) 
volatile float WR_ref = 0.0;
static const float Kp_L = 0.2; //0.11 //without loading
static const float Kp_R = 0.2;
static const float Ki_L = 0.2;
static const float Ki_R = 0.2;
volatile float integralL = 0.0;
volatile float integralR = 0.0;
static const float integral_dec = 0.99; // for position control, it should be 1.0
static const float integral_max = 400;
static const float integral_min = -400;


      integralR = integral_max;
    if(integralR < integral_min)
      integralR = integral_min;
    float der_error = (error - prev_error_R)/timer_dt;  
    prev_error_R = error;
    cmd = feedforward(WR_ref) + error*Kp_R + integralR*Ki_R + der_error*Kd_R; // controller
    if(cmd>=0.0)
    {
      analogWrite(out3_pin, (int)cmd);
