Diff: main.cpp

Revision:

2:e87736742f99

Parent:

1:9149cfedd4d5

Child:

3:246625f3ceee

--- a/main.cpp	Sun Apr 15 20:03:35 2018 +0000
+++ b/main.cpp	Mon Apr 16 12:19:39 2018 +0000
@@ -36,6 +36,7 @@
 
 Serial bt(PTE0, PTE1); //COM12
 DigitalOut myled(LED_BLUE);
+DigitalOut brake(PTD0);
 
 Ticker control;
 Timer ctrlTimer;
@@ -48,43 +49,68 @@
 float Kps = 2.0E-2; //0.013 for setpoint = 0.0/0.05
 float Kd = 1.0e-4;
 
+void display()
+{
+    bt.printf("Setpoint = %f, Kps = %f, Kd = %f, Brake = %f\r\n", Setpoint, Kps, Kd, brake.read());
+}
+
 void serCb()
 {
     char x = bt.getc();
     if (x == 'u')
     {
-        Setpoint = Setpoint + 0.05;
+        Setpoint += 0.05;
+        display();
     }
     else if(x == 'h')
     {
-        Setpoint = Setpoint - 0.05;
+        Setpoint -= 0.05;
+        display();
     }
     else if (x == 'i')
     {
         Kps += 1.0e-3;
+        display();
     }
     else if (x == 'j')
     {
         Kps -= 1.0e-3;
+        display();
     }
     else if (x == 'o')
     {
         Kd += 1.0e-5;
+        display();
     }
     else if (x == 'k')
     {
         Kd -= 1.0e-5;
+        display();
     }
-    
+    else if (x == 'b')
+    {
+        brake.write(1);
+        display();
+    }
+    else if (x == 'g')
+    {
+        brake.write(0);
+        display();
+    }
+    else if (x == 'p')
+    {
+        display();
+    }    
     else
     {
-        bt.putc(x);
+        bt.printf("Invalid input");
     }
     if(Setpoint > MAXM) Setpoint = MAXM;
-    if(Setpoint < MINM) Setpoint = MINM;
-    bt.printf("Setpoint = %f, Kps = %f, Kd = %f\r\n", Setpoint, Kps, Kd);    
+    if(Setpoint < MINM) Setpoint = MINM;    
 }
 
+
+
 void stop()
 {
   Setpoint = 0.0f;  
@@ -128,11 +154,11 @@
     if(output < MINM) 
     {
         gateDrive.write(MINM);
-        //brake.write(1);
+        brake.write(1);
     }
     else 
     {
-        //brake.write(0);
+        brake.write(0);
         gateDrive.write(output);
     }     
     data[4] = gateDrive.read();
@@ -157,7 +183,7 @@
     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]);