   Serial.print(th);
   Serial.print(" av: ");
   Serial.print(av);
-  Serial.print(" gv: ");
-  Serial.println(gv);
-  motion(lpwm,rpwm,HIGH,HIGH);
+  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...
 

  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

  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);
