     analogWrite(out3_pin, 0);
     analogWrite(out4_pin, 0);
   }
+
+  
   
   
   // This is a workaround to prevent from motor idle when speed  = 0;

      integralL = integral_max;
    if(integralL < integral_min)
      integralL = integral_min;
    cmd = feedforward(WL_ref) + error*Kp_L + integralL*Ki_L; // controller
    Serial.println(cmd);
    if(cmd>=0.0)
    {
      analogWrite(out1_pin, (int)cmd);

  int pin2_counter = decoder_pin_2_counter;
  
  // average filter of encoder data
  total_pin1 = total_pin1 - (float)filter_pin1[filterId];
  total_pin2 = total_pin2 - (float)filter_pin2[filterId];
  filter_pin1[filterId] = pin1_counter;
  filter_pin2[filterId] = pin2_counter;
  total_pin1 = total_pin1 + (float)filter_pin1[filterId];
  total_pin2 = total_pin2 + (float)filter_pin2[filterId];
  filterId = filterId + 1;
  if(filterId >= filter_size)
    filterId = 0;
  
  float average_counter_pin1 = total_pin1/((float)filter_size);
  float average_counter_pin2 = total_pin2/((float)filter_size);
  //Serial.println(average_counter_pin2,4); //debug

  // controller (feedforward + feedback)
  float cmd = 0.0;
  float error = 0.0;
