   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;  
-}
-
-
-
+

*/
#include <MsTimer2.h>

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 = 2;
const int decoder_pin_2 = 3;
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;

// Report internal;
const int report_period = 100;


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

  decoder_pin_2_counter += 1;
}

// convert required deg/s to pwm command (based on statistical)
float feedforward(float value) {
  return 0.1335 * value + 7.07;  
}

void controller_repoter_isr() {
  // controller (feedforward + feedback)
  float cmd = 0.0;
  float error = 0.0;
  if(WL_ref >= 0.0)
  {
    error = WL_ref - (float)(decoder_pin_1_counter*encoder_res)*timer_hz; // deg/sec
    cmd = feedforward(WL_ref) + error*Kp_L;
    if(cmd>=0.0)
    {
      analogWrite(out1_pin, (int)cmd);
      analogWrite(out2_pin, 0);
    }
    else
    {
      analogWrite(out1_pin, 0);
      analogWrite(out2_pin, -(int)cmd);
    }
  }
  else // the encoder has no capability of distinguishing the cw/ccw rotation
  {
    error = -WL_ref - (float)(decoder_pin_1_counter*encoder_res)*timer_hz; // deg/sec
    cmd = feedforward(WL_ref) + error*Kp_L;
    if(cmd>=0.0)
    {
      analogWrite(out2_pin, (int)cmd);
      analogWrite(out1_pin, 0);
    }
    else
    {
      analogWrite(out2_pin, 0);
      analogWrite(out1_pin, -(int)cmd);
    }
  }

  if(WR_ref >= 0.0)
  {
    error = WR_ref - (float)(decoder_pin_2_counter*encoder_res)*timer_hz; // deg/sec
    cmd = feedforward(WR_ref) + error*Kp_R;
    if(cmd>=0.0)
    {
      analogWrite(out3_pin, (int)cmd);
      analogWrite(out4_pin, 0);
    }
    else
    {
      analogWrite(out3_pin, 0);
      analogWrite(out4_pin, -(int)cmd);
    }
  }
  else // the encoder has no capability of distinguishing the cw/ccw rotation
  {
    error = -WR_ref - (float)(decoder_pin_2_counter*encoder_res)*timer_hz; // deg/sec
    cmd = feedforward(WR_ref) + error*Kp_R;
    if(cmd>=0.0)
    {
      analogWrite(out4_pin, (int)cmd);
      analogWrite(out3_pin, 0);
    }
    else
    {
      analogWrite(out4_pin, 0);
      analogWrite(out3_pin, -(int)cmd);
    }
  }
  
  
  // This is a workaround to prevent from motor idle when speed  = 0;
  if (decoder_pin_1_delay_counter > (100 + WR_ref/encoder_res/timer_hz) ) {
    WR_ref = 0.0;
    analogWrite(out3_pin, 0);
    analogWrite(out4_pin, 0);
    decoder_pin_1_delay_counter = 0;
  }
  
  if (decoder_pin_2_delay_counter > (100 + WL_ref/encoder_res/timer_hz) ) {
    WL_ref = 0.0;
    analogWrite(out1_pin, 0);
    analogWrite(out2_pin, 0);
    decoder_pin_2_delay_counter = 0;
  }

  // report encoder info (deg/sec)
  char str[16];
  sprintf(str, "%d,%d\r\n", decoder_pin_1_counter*encoder_res
                          , decoder_pin_2_counter*encoder_res ); // deg/sec
  Serial.print(str);

  if (decoder_pin_1_counter == 0)
