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

void loop() {
    if (Serial.available()) {
      String commad_string = Serial.readString();
      String out_1 = getValue(commad_string, ',', 0); //getValue func should be removed (TBD) 
      String out_2 = getValue(commad_string, ',', 1);
      String out_3 = getValue(commad_string, ',', 2);
      String out_4 = getValue(commad_string, ',', 3);
      if (out_1 != NULL)
        WL_ref =  out_1.toFloat(); // deg/sec
        
      if (out_2 != NULL)
        WL_ref = -out_2.toFloat();
         
      if (out_3 != NULL)
        WR_ref =  out_3.toFloat();
        
      if (out_4 != NULL)
        WR_ref = -out_4.toFloat();
    }
}



// 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() {
  // copy current counter
  int pin1_counter = decoder_pin_1_counter;
  int pin2_counter = decoder_pin_2_counter;
  
  // average filter of encoder data
  float average_counter_pin1 = 0.0;
  for(int i=0; i<filter_size-1; i++)
  {
    average_filter_pin1[i] = average_filter_pin1[i+1];
    average_counter_pin1 = average_counter_pin1 + average_filter_pin1[i+1];
  }  
  average_filter_pin1[filter_size-1] = pin1_counter;
  average_counter_pin1 = average_counter_pin1 + pin1_counter;
  average_counter_pin1 = average_counter_pin1/(float)filter_size;

  float average_counter_pin2 = 0.0;
  for(int i=0; i<filter_size-1; i++)
  {
    average_filter_pin2[i] = average_filter_pin2[i+1];
    average_counter_pin2 = average_counter_pin2 + average_filter_pin2[i+1];
  }  
  average_filter_pin2[filter_size-1] = pin2_counter;
  average_counter_pin2 = average_counter_pin2 + pin2_counter;
  average_counter_pin2 = average_counter_pin2/(float)filter_size;
  
  // controller (feedforward + feedback)
  float cmd = 0.0;
  float error = 0.0;
  if(WL_ref > 0.0)
  {
    error = WL_ref - average_counter_pin1*(float)(encoder_res)*timer_hz; // deg/sec
    cmd = feedforward(WL_ref) + error*Kp_L;
    if(cmd>=0.0)
    {
