       integralR = integral_max;
     if(integralR < integral_min)
       integralR = integral_min;
-    cmd = feedforward(-WR_ref) + error*Kp_R + integralR*Ki_R; // controller
+    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(out4_pin, (int)cmd);

// convert required deg/s to pwm command (based on statistical)
float feedforward(float value) {
  //return 0.1335 * value + 7.07; //withous laoding
    return 0.4 * value;
}

void controller_repoter_isr() {

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

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