   else if(WR_ref < 0.0)// the encoder has no capability of distinguishing the cw/ccw rotation
   {
     error = -WR_ref - average_counter_pin2*(float)(encoder_res)*timer_hz; // deg/sec
-    cmd = feedforward(-WR_ref) + error*Kp_R;
+    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(out4_pin, (int)cmd);

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

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

#include <MsTimer2.h>

// Pin define
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
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;
