 static const int decoder_pin_2 = 2;
 
 // Timer internal
-static const int timer_period = 50; // ms 
+static const int timer_period = 20; // ms 
 static const float timer_hz = 1000.0/( (float)timer_period ); // hz
 static const float timer_dt = timer_period/1000.0; // sec
-static const int filter_size = 10;
+static const int filter_size = 20;
 static const int counter_protect = 200;
 
 // encoder

    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);
    }
    else
    {
      analogWrite(out3_pin, 0);
      analogWrite(out4_pin, -(int)cmd);
    }
  }
  else if(WR_ref < 0.0)// the encoder has no capability of distinguishing the cw/ccw rotation

// controller
volatile float WL_ref = 0.0; //reference speed for left wheel (deg/sec) 
volatile float WR_ref = 0.0;
static const float Kp_L = 0.15;
static const float Kp_R = 0.15;
static const float Ki_L = 0.45; //since the left motor of demo bot is weaker than the right
static const float Ki_R = 0.4;
static const float Kd_L = 0.01;
static const float Kd_R = 0.01;
volatile float integralL = 0.0;
volatile float integralR = 0.0;
volatile float prev_error_L = 0.0;
volatile float prev_error_R = 0.0;
static const float integral_dec = 1.0; // for position control, it should be 1.0
static const float integral_max = 1000;
static const float integral_min = -1000;
static const float Kp_tilted = 0.0;

void setup() {
  // Set 4 pwm channel pin to output
