   return found>index ? data.substring(strIndex[0], strIndex[1]) : "";
 }
 
-// convert required deg/s to pwm command (feedforward)
-float convert_adc(int value) {
-  return 0.1335 * value + 7.07;  
-}
-
-
-
+

  attachInterrupt(digitalPinToInterrupt(decoder_pin_1), decoder_1_isr, FALLING);
  attachInterrupt(digitalPinToInterrupt(decoder_pin_2), decoder_2_isr, FALLING);

  // Start timer for reporting wheel speed
  MsTimer2::set(report_period, report_decoder_result_isr); // 100ms period
  MsTimer2::start();
}

void loop() {
    if (Serial.available()) {
      String commad_string = Serial.readString();
      String out_1 = getValue(commad_string, ',', 0);
      String out_2 = getValue(commad_string, ',', 1);
      String out_3 = getValue(commad_string, ',', 2);
      String out_4 = getValue(commad_string, ',', 3);
      if (out_1 != NULL) {
        int out1_value = out_1.toFloat();
        analogWrite(out1_pin, convert_adc(out1_value));
      }
      if (out_2 != NULL) {
        int out2_value = out_2.toFloat();
        analogWrite(out2_pin, convert_adc(out2_value));
      }
      if (out_3 != NULL) {
        int out3_value = out_3.toFloat();
        analogWrite(out3_pin, convert_adc(out3_value));
      }
      if (out_4 != NULL) {
        int out4_value = out_4.toFloat();
        analogWrite(out4_pin, convert_adc(out4_value));
      }
    }
}


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

// Timer internal
const int timer_period = 50; // ms 
const float timer_hz = 1000.0/( (float)timer_period ); // hz

// encoder
volatile int decoder_pin_1_counter = 0;
volatile int decoder_pin_2_counter = 0;
volatile int decoder_pin_1_delay_counter = 0;
volatile int decoder_pin_2_delay_counter = 0;
volatile int encoder_res = 18; // 20 pulse in a circle --> 360/20 = 18(deg/pulse)

// 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.1;
volatile float Kp_R = 0.1;

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