 // convert required deg/s to pwm command (based on statistical)
 float feedforward(float value) {
   //return 0.1335 * value + 7.07; //withous laoding
-    return 0.5 * value;
+    return 0.4 * value;
 }
 
 void controller_repoter_isr() {
   // copy current counter
-  int pin1_counter = decoder_pin_1_counter;
-  int pin2_counter = decoder_pin_2_counter;
+  int pin1_counter = decoder_pin_1_counter; // Left
+  int pin2_counter = decoder_pin_2_counter; // Right
   
   // average filter of encoder data
   total_pin1 = total_pin1 - (float)filter_pin1[filterId];

  // controller (feedforward + feedback)
  float cmd = 0.0;
  float error = 0.0;
  if(WL_ref > 0.0)
  {
    error = WL_ref - average_counter_pin1*(float)(encoder_res)*timer_hz; // deg/sec
    integralL = integralL*integral_dec + error*timer_dt;

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