   if(WR_ref > 0.0)
   {
     error = WR_ref - average_counter_pin2*(float)(encoder_res)*timer_hz; // deg/sec
-    cmd = feedforward(WR_ref) + error*Kp_R;
+    integralR = integralR*integral_dec + error*timer_dt;
+    if(integralR > integral_max)
+      integralR = integral_max;
+    if(integralR < integral_min)
+      integralR = integral_min;
+    cmd = feedforward(WR_ref) + error*Kp_R + integralR*Ki_R; // controller
     if(cmd>=0.0)
     {
       analogWrite(out3_pin, (int)cmd);

  else if(WR_ref < 0.0)// the encoder has no capability of distinguishing the cw/ccw rotation
  {
    error = -WR_ref - average_counter_pin2*(float)(encoder_res)*timer_hz; // deg/sec
    cmd = feedforward(-WR_ref) + error*Kp_R;
    if(cmd>=0.0)
    {
      analogWrite(out4_pin, (int)cmd);

  else if(WL_ref < 0.0) // the encoder has no capability of distinguishing the cw/ccw rotation
  {
    error = -WL_ref - average_counter_pin1*(float)(encoder_res)*timer_hz; // deg/sec
    integralL = integralL*integral_dec + error*timer_dt;
    if(integralL > integral_max)
      integralL = integral_max;
    if(integralL < integral_min)
      integralL = integral_min;
    cmd = feedforward(-WL_ref) + error*Kp_L + integralL*Ki_L; // controller
    if(cmd>=0.0)
    {
      analogWrite(out2_pin, (int)cmd);
