     
     if(cmd>=0.0)
     {
-      analogWrite(out2_pin, (int)cmd);
-      analogWrite(out1_pin, 0);
+      pin2_pwm = (int)cmd;
+      pin1_pwm = 0;
     }
     else
     {
-      analogWrite(out2_pin, 0);
-      analogWrite(out1_pin, -(int)cmd);
+      pin2_pwm = 0;
+      pin1_pwm = -(int)cmd;
     }
   }
   else // == 0
   {
     integralL = 0.0;
-    analogWrite(out1_pin, 0);
-    analogWrite(out2_pin, 0);
-  }
-
-  if(WR_ref > 0.0)
+    pin1_pwm = 0;
+    pin2_pwm = 0;
+  }
+
+  if(WR_ref > 0.0) //Right wheel forward
   {
     error = WR_ref - average_counter_pin2*(float)(encoder_res)*timer_hz; // deg/sec
     integralR = integralR*integral_dec + error*timer_dt;

    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

static const int decoder_pin_2 = 2;

// Timer internal
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 = 20;
static const int counter_protect = 200;

// encoder
