 int ltp=0;      // left motor target position 
 float av, th;   // average & angular velocity
 int ti=0;
-int gv=8;        // goal velocity
-float kpl=2;     // Kp from PID
-float kpr=2;     // Kp from PID
+int gv=0;        // goal velocity
+int period=100;    // timer period in ms
+double lIn,rIn,tIn,lOut,rOut,tOut,lSet,rSet,tSet;
+double lkp=1,lki=0,lkd=0;     // Left wheel PID constants
+double rkp=1,rki=0,rkd=0;     // Right wheel PID constants
+double tkp=1,tki=0,tkd=0;     // Right wheel PID constants
+
+//Specify the links and initial tuning parameters
+//PID(&Input, &Output, &Setpoint, Kp, Ki, Kd, Direction)
+PID lPID(&lIn, &lOut, &lSet,lkp,lki,lkd, DIRECT);
+PID rPID(&rIn, &rOut, &rSet,rkp,rki,rkd, DIRECT);
+PID tPID(&tIn, &tOut, &tSet,tkp,tki,tkd, DIRECT);
 
 os_timer_t myTimer;
 
+
+void pidupdate(){
+  rPID.Compute();
+  lPID.Compute();
+}
 // start of timerCallback
 void tic(void *pArg) {
   lv=lmc-lmc0;            // lv left instant velocity

  rmc0=rmc;
  th=rmc-lmc;             // angular position(*pi()/rstep(180))
  av=(lv+rv)/2.;          // center forward velocity
  ti+=1;                  // tic counter 
  if(lv>gv){ lpwm-=(lv-gv)*kpl;}
  if(lv<gv){ lpwm+=(gv-lv)*kpl;}
  if(rv>gv){ rpwm-=(rv-gv)*kpr;}
  if(rv<gv){ rpwm+=(gv-rv)*kpr;}
}

void stop(void){      // Stop both motors

  Serial.print(th);
  Serial.print(" av: ");
  Serial.print(av);
  Serial.print(" lSet: ");
  Serial.print(lSet);
  Serial.print(" lIn: ");
  Serial.print(lIn);
  Serial.print(" lOut: ");
  Serial.print(lOut);
  Serial.print(" lkp: ");
  Serial.print(lkp);
  Serial.print(" lki: ");
  Serial.print(lki);
  Serial.print(" lkd: ");
  Serial.println(lkd);
  pidupdate();
  delay(1000);
}//read int or parseFloat for ..float...

