       if(c=='s'){
         lSet=gv/10.;
         rSet=gv/10.;
-      }
-      if(c=='p'){
+      } else if(c=='p'){
         lkp=gv/10.;
         rkp=gv/10.;
-      }
-      if(c=='i'){
+        lPID.SetTunings(lkp, lki, lkd);
+        rPID.SetTunings(rkp, rki, rkd);
+      } else if(c=='i'){
         lki=gv/10.;
         rki=gv/10.;
-      }
-      if(c=='d'){
+        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;
+      //lPID.SetMode(MANUAL);
+      //rPID.SetMode(MANUAL);
+      //lPID.SetSampleTime(period);
+      //rPID.SetSampleTime(period); 
+      //lPID.SetOutputLimits(0, 1023);  
+      //rPID.SetOutputLimits(0, 1023);  
+      //lPID.SetMode(AUTOMATIC);
+      //rPID.SetMode(AUTOMATIC);
     }
   }
   Serial.print(" lSet: ");

double whesep= 0.135; // wheel separtion in m
double whedia= 0.7;   // wheel diameter in m
int CPR = 40;     // Encoder Count per Revolutions (double of holes using up & down interrupt)
int period=200;   // sample timer period in ms
double lv=0, rv=0;// motor speed innumber tics per period
int ti=0;         // tic timer counter
double lOut,rOut,lSet,rSet;   // PID output and demands
double lkp=1,lki=0,lkd=0;     // Left wheel PID constants
double rkp=1,rki=0,rkd=0;     // Right wheel PID constants
int kt=1000/period; // number of periods in 1sec

//PID(&Input, &Output, &Setpoint, Kp, Ki, Kd, Direction)
PID lPID(&lv, &lOut, &lSet, lkp, lki, lkd, DIRECT);

  Serial.print(" rSet: ");
  Serial.print(rSet);
  Serial.print(" lv:");
  Serial.print(lv*1000);
  Serial.print(" rv:");
  Serial.print(rv*1000);
  Serial.print(" lOut:"); 
  Serial.print(lOut);
  Serial.print(" rOut:");
  Serial.print(rOut);
  Serial.print(" lmc:");
  Serial.print(lmc);
  Serial.print(" rmc=");
