     analogWrite(D2, 0);
 }
  
-void motion(double lpwm, double rpwm, int llevel, int rlevel) {  
-    if (llevel==HIGH) {
+void motion(double lpwm, double rpwm) {  
+  if(abs(lIn)<1){
+    if(lOut>=0){
       ldir=1; 
+      llevel=HIGH; 
     } else {
       ldir=-1;
-    }
-    if (rlevel==HIGH) {
+      llevel=LOW; 
+    }
+  }
+  if(abs(rIn)<1){
+    if(rOut>=0){
       rdir=1; 
+      rlevel=HIGH; 
     } else {
       rdir=-1;
-    }
-    if(lpwm>1023)lpwm=1023;
-    if(rpwm>1023)rpwm=1023;
-    if(lpwm<0)lpwm=0;
-    if(rpwm<0)rpwm=0;
-    analogWrite(D1, lpwm);
-    analogWrite(D2, rpwm);
-    digitalWrite(D3, llevel);
-    digitalWrite(D4, rlevel);
+      rlevel=LOW; 
+    }
+  }
+  analogWrite(D1, abs(lpwm));
+  analogWrite(D2, abs(rpwm));
+  digitalWrite(D3, llevel);
+  digitalWrite(D4, rlevel);
 }
 void lencode() {          // GPIO ISR Interrupt service routines for encoder changes
   lcurrenttime = millis();
-  lvt = 1000./(lcurrenttime - llasttime);
+  lvt = ldir*1000./(lcurrenttime - llasttime);
   llasttime = lcurrenttime;
   lmc=lmc+ldir;
 }
 void rencode(){ 
   rcurrenttime = millis();
-  rvt = 1000./(rcurrenttime - rlasttime);
+  rvt = rdir*1000./(rcurrenttime - rlasttime);
   rlasttime = rcurrenttime;
   rmc=rmc+rdir;
 }
 
-
-void setup() {
-  pinMode(D1, OUTPUT); // 1,2EN aka D1 pwm left
-  pinMode(D2, OUTPUT); // 3,4EN aka D2 pwm right
-  pinMode(D3, OUTPUT); // 1A,2A aka D3
-  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 
-  attachInterrupt(D5, lencode, RISING);   // Setup Interrupt 
-  attachInterrupt(D6, rencode, RISING);   // Setup Interrupt 
-  sei();                                  // Enable interrupts  
-  os_timer_setfn(&myTimer, tic, NULL);
-  os_timer_arm(&myTimer, period, true);   // timer in ms
-  lPID.SetSampleTime(period);
-  rPID.SetSampleTime(period); 
-  lPID.SetOutputLimits(0, 1023);  
-  rPID.SetOutputLimits(0, 1023);  
-  lPID.SetMode(AUTOMATIC);
-  rPID.SetMode(AUTOMATIC);
-  Serial.begin(115200);
-  Serial.println("Ready");
-}
-
-char c=0;             // input char from keys
-void loop(){
-  // watch for input
-  if(c==0){
-    if (Serial.available() != 0) {
-      c = Serial.read();
-      Serial.println(c);
-    }
-  } else {
-    if (Serial.available() != 0) {
-      int gv = Serial.parseInt();
-      if(c=='s'){
-        lSet=gv/10.;
-        rSet=gv/10.;
-      } else if(c=='p'){
-        lkp=gv/10.;
-        rkp=gv/10.;
-        lPID.SetTunings(lkp, lki, lkd);
-        rPID.SetTunings(rkp, rki, rkd);
-      } else if(c=='i'){
-        lki=gv/10.;
-        rki=gv/10.;
-        lPID.SetTunings(lkp, lki, lkd);
-        rPID.SetTunings(rkp, rki, rkd);
-      } else if(c=='d'){
-        lkd=gv/10.;
-        rkd=gv/10.;
-        lPID.SetTunings(lkp, lki, lkd);
-        rPID.SetTunings(rkp, rki, rkd);
-      } else {
-        Serial.println("Tunning command not recognized. Use s NNN, p NNN, i NNN, d NNN with NNN=n*10");
-      }
-      while (Serial.available()) Serial.read();
-      c=0;
-    }
-  }
-  /*
-  Serial.print("***lSet: ");
+void dump(){
+  Serial.print("lSet: ");
   Serial.print(lSet);
   Serial.print(" rSet: ");
   Serial.print(rSet);
-  Serial.print(" lv:");
+  Serial.print(" lIn:");
   Serial.print(lIn);
-  Serial.print(" rv:");
+  Serial.print(" rIn:");
   Serial.print(rIn);
   Serial.print(" lOut:"); 
   Serial.print(lOut);

  Serial.print(lki);
  Serial.print(" lkd: ");
  Serial.println(lkd);
*/
  delay(1000);
}


PID rPID(&rIn, &rOut, &rSet, rkp, rki, rkd, DIRECT);

os_timer_t myTimer;

static inline int8_t sgn(int val) {
 if (val < 0) return -1;
 if (val==0) return 0;
 return 1;
}

// start of timerCallback, repeat every "period"
void tic(void *pArg) {
