     cmd = feedforward(WR_ref) + error*Kp_R + integralR*Ki_R + der_error*Kd_R; // controller
     if(cmd>=0.0)
     {
-      analogWrite(out3_pin, (int)cmd);
-      analogWrite(out4_pin, 0);
+      pin3_pwm = (int)cmd;
+      pin4_pwm = 0;
     }
     else
     {
-      analogWrite(out3_pin, 0);
-      analogWrite(out4_pin, -(int)cmd);
+      pin3_pwm = 0;
+      pin4_pwm = -(int)cmd;
     }
   }
   else if(WR_ref < 0.0)// the encoder has no capability of distinguishing the cw/ccw rotation

    cmd = feedforward(-WR_ref) + error*Kp_R + integralR*Ki_R + der_error*Kd_R; // controller
    if(cmd>=0.0)
    {
      analogWrite(out4_pin, (int)cmd);
      analogWrite(out3_pin, 0);
    }
    else
    {
      analogWrite(out4_pin, 0);
      analogWrite(out3_pin, -(int)cmd);
    }
  }
  else // == 0
  {
    integralR = 0.0;
    analogWrite(out3_pin, 0);
    analogWrite(out4_pin, 0);
  }

  
  
  
  // This is a workaround to prevent from motor idle when speed  = 0;
  if (decoder_pin_1_delay_counter > counter_protect ) {

  // controller (feedforward + feedback)
  float cmd = 0.0;
  float error = 0.0;
  int pin1_pwm = 0;
  int pin2_pwm = 0;
  int pin3_pwm = 0;
  int pin4_pwm = 0;
  
  if(WL_ref > 0.0) //Left wheel forward
  {
    error = WL_ref - average_counter_pin1*(float)(encoder_res)*timer_hz; // deg/sec
    integralL = integralL*integral_dec + error*timer_dt;
