       analogWrite(out2_pin, -(int)cmd);
     }
   }
-  else // the encoder has no capability of distinguishing the cw/ccw rotation
-  {
-    error = -WL_ref - (float)(decoder_pin_1_counter*encoder_res)*timer_hz; // deg/sec
-    cmd = feedforward(WL_ref) + error*Kp_L;
+  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
+    cmd = feedforward(-WL_ref) + error*Kp_L;
     if(cmd>=0.0)
     {
       analogWrite(out2_pin, (int)cmd);

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

      analogWrite(out3_pin, -(int)cmd);
    }
  }
  else // == 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 ) {
    WR_ref = 0.0;
    analogWrite(out3_pin, 0);
    analogWrite(out4_pin, 0);
    decoder_pin_1_delay_counter = 0;
  }
  
  if (decoder_pin_2_delay_counter > counter_protect ) {
    WL_ref = 0.0;
    analogWrite(out1_pin, 0);
    analogWrite(out2_pin, 0);
