Diff: main.cpp

Revision:

1:9149cfedd4d5

Parent:

0:30871514c229

Child:

2:e87736742f99

--- a/main.cpp	Thu Apr 12 21:07:30 2018 +0000
+++ b/main.cpp	Sun Apr 15 20:03:35 2018 +0000
@@ -11,9 +11,9 @@
 
 #define TI 0.001f
 
-#define SCALAR 0.6f
+#define SCALAR 0.53f
 #define MINM 0.0f
-#define MAXM 1.0f
+#define MAXM 0.53f
 #define KPM 0.1414f
 #define KI 19.7408f
 
@@ -45,6 +45,8 @@
 float errSum = 0.0;
 
 float fbPrev = 0.0f;
+float Kps = 2.0E-2; //0.013 for setpoint = 0.0/0.05
+float Kd = 1.0e-4;
 
 void serCb()
 {
@@ -57,13 +59,30 @@
     {
         Setpoint = Setpoint - 0.05;
     }
+    else if (x == 'i')
+    {
+        Kps += 1.0e-3;
+    }
+    else if (x == 'j')
+    {
+        Kps -= 1.0e-3;
+    }
+    else if (x == 'o')
+    {
+        Kd += 1.0e-5;
+    }
+    else if (x == 'k')
+    {
+        Kd -= 1.0e-5;
+    }
+    
     else
     {
         bt.putc(x);
     }
     if(Setpoint > MAXM) Setpoint = MAXM;
     if(Setpoint < MINM) Setpoint = MINM;
-    bt.printf("%f\r\n", Setpoint);     
+    bt.printf("Setpoint = %f, Kps = %f, Kd = %f\r\n", Setpoint, Kps, Kd);    
 }
 
 void stop()
@@ -81,7 +100,7 @@
     }    
     float fb = L - R; 
     float e = SET - fb;
-    float Controlleroutput = KPS * e - (KD * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error?? 
+    float Controlleroutput = Kps * e - (Kd * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error?? 
     if (Controlleroutput > MAXS) Controlleroutput = MAXS;
     else if (Controlleroutput < MINS) Controlleroutput = MINS;
     if (abs(Controlleroutput - servoSig.read()) > TOL || ctrlTimer.read() >= STEERUPDATEPERIOD) 
@@ -138,14 +157,14 @@
     control.attach(&cb, TI);
     
     bt.baud(115200);
-    bt.printf("Right Left SteerControl SteerError GateDrive Speed\r\n");
+    //bt.printf("Right Left SteerControl SteerError GateDrive Speed\r\n");
     while(1) {
         myled = !myled;
-        bt.printf("%f ",data[0]);
-        bt.printf("%f ", data[1]);
-        bt.printf("%f ", data[2]);
-        bt.printf("%f ", data[3]);
-        bt.printf("%f ", data[4]);
-        bt.printf("%f\r\n", data[5]);
+        //bt.printf("%f ",data[0]);
+//        bt.printf("%f ", data[1]);
+//        bt.printf("%f ", data[2]);
+//        bt.printf("%f ", data[3]);
+//        bt.printf("%f ", data[4]);
+//        bt.printf("%f\r\n", data[5]);
     }
 }
\ No newline at end of file