 // controller
 volatile float WL_ref = 0.0; //reference speed for left wheel (deg/sec) 
 volatile float WR_ref = 0.0;
-volatile float Kp_L = 0.2; //0.11 //without loading
-volatile float Kp_R = 0.2;
+static const float Kp_L = 0.2; //0.11 //without loading
+static const float Kp_R = 0.2;
+static const float Ki_L = 0.2;
+static const float Ki_R = 0.2;
+volatile float integralL = 0.0;
+volatile float integralR = 0.0;
+static const float integral_dec = 0.99; // for position control, it should be 1.0
+static const float integral_max = 400;
+static const float integral_min = -400;
 
 void setup() {
   // Set 4 pwm channel pin to output

  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;
    if(cmd>=0.0)
    {
      analogWrite(out3_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);
