 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

#include <ESP8266WiFi.h>
extern "C" {
#include "user_interface.h"
}

  pinMode(D4, OUTPUT); // 3A,4A aka D4
  pinMode(D5, INPUT); //  Left encoder
  pinMode(D6, INPUT); //  Right encoder
  attachInterrupt(D5, lencode, CHANGE); // Setup Interrupt 
  attachInterrupt(D6, rencode, CHANGE); // Setup Interrupt 
  sei();                                // Enable interrupts  
  int currentTime = millis();
  int cloopTime = currentTime;
  os_timer_setfn(&myTimer, tic, NULL);
  os_timer_arm(&myTimer, period, true);   // timer in ms
  lPID.SetSampleTime(period);
  rPID.SetSampleTime(period); 
  tPID.SetSampleTime(period);  
  lPID.SetMode(AUTOMATIC);
  rPID.SetMode(AUTOMATIC);
  tPID.SetMode(AUTOMATIC);
  Serial.begin(115200);
  Serial.println("Ready");
}
char c=0;
void loop(){
  // watch for input
  if(c==0){
    if (Serial.available() != 0) {
      c = Serial.read();
      Serial.println(c);
    }
  } else {
    if (Serial.available() != 0) {
      gv = Serial.parseInt();
      if(c=='s'){
        lSet=gv/10.;
        rSet=gv/10.;
      }
      if(c=='p'){
        lkp=gv/10.;
        rkp=gv/10.;
      }
      if(c=='i'){
        lki=gv/10.;
        rki=gv/10.;
      }
      if(c=='d'){
        lkd=gv/10.;
        rkd=gv/10.;
      }
      while (Serial.available()) Serial.read();
      c=0;
    }
  }
  Serial.print("lv:rv=");
  Serial.print(lv);
