 #include <MsTimer2.h>
 
 // Pin define
-const int out1_pin = 5;
-const int out2_pin = 6;
-const int out3_pin = 9;
-const int out4_pin = 10;
-const int decoder_pin_1 = 3;
-const int decoder_pin_2 = 2;
+static const int out1_pin = 5;
+static const int out2_pin = 6;
+static const int out3_pin = 9;
+static const int out4_pin = 10;
+static const int decoder_pin_1 = 3;
+static const int decoder_pin_2 = 2;
 
 // Timer internal
-const int timer_period = 50; // ms 
-const float timer_hz = 1000.0/( (float)timer_period ); // hz
-const int filter_size = 5;
-const int counter_protect = 200;
+static const int timer_period = 50; // 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 = 5;
+static const int counter_protect = 200;
 
 // encoder
 volatile int decoder_pin_1_counter = 0;

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

  if(WR_ref > 0.0)
  {
    error = WR_ref - average_counter_pin2*(float)(encoder_res)*timer_hz; // deg/sec
    integralR = integralR*integral_dec + error*timer_dt;
    if(integralR > integral_max)
      integralR = integral_max;
    if(integralR < integral_min)
      integralR = integral_min;
    cmd = feedforward(WR_ref) + error*Kp_R + integralR*Ki_R; // controller
    if(cmd>=0.0)
    {
      analogWrite(out3_pin, (int)cmd);
