Diff: main.cpp

Revision:

3:246625f3ceee

Parent:

2:e87736742f99

Child:

4:af9973350ffe

--- a/main.cpp	Mon Apr 16 12:19:39 2018 +0000
+++ b/main.cpp	Tue Apr 17 22:56:24 2018 +0000
@@ -14,7 +14,7 @@
 #define SCALAR 0.53f
 #define MINM 0.0f
 #define MAXM 0.53f
-#define KPM 0.1414f
+#define KPM 0.15f //0.1414f
 #define KI 19.7408f
 
 #define KPS 2.0E-2f //Original 2.0e-2
@@ -40,18 +40,28 @@
 
 Ticker control;
 Timer ctrlTimer;
+
+InterruptIn navRt(PTD2);
+InterruptIn navLft(PTD3);
+
+volatile int rightCount;
+volatile int leftCount;
+
 float data[6];
 
-float Setpoint = 0.0f;
+float Setpoint = 0.0;
+//float sp = 0.05;
 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;
 
+float Kpm = 0.141;
+
 void display()
 {
-    bt.printf("Setpoint = %f, Kps = %f, Kd = %f, Brake = %f\r\n", Setpoint, Kps, Kd, brake.read());
+    bt.printf("Setpoint = %f, Kps = %f, Kd = %f, Kpm = %f, Brake = %f\r\n", Setpoint, Kps, Kd, Kpm, brake.read());
 }
 
 void serCb()
@@ -59,12 +69,12 @@
     char x = bt.getc();
     if (x == 'u')
     {
-        Setpoint += 0.05;
+        Setpoint += 0.025;
         display();
     }
     else if(x == 'h')
     {
-        Setpoint -= 0.05;
+        Setpoint -= 0.025;
         display();
     }
     else if (x == 'i')
@@ -91,8 +101,9 @@
     {
         brake.write(1);
         display();
+       
     }
-    else if (x == 'g')
+    else if (x == 'n')
     {
         brake.write(0);
         display();
@@ -100,7 +111,21 @@
     else if (x == 'p')
     {
         display();
-    }    
+    }
+    else if (x == 'y')
+    {
+        Kpm += 0.003;
+        display();   
+    }   
+    else if(x == 'g')
+    {
+        Kpm -= 0.003; 
+        display();   
+    } 
+    else if (x == 'z')
+    {
+        bt.printf("Right = %i Left = %i\r\n", rightCount, leftCount);
+    }
     else
     {
         bt.printf("Invalid input");
@@ -110,20 +135,25 @@
 }
 
 
+void incL()
+{      
+    leftCount++;
+}
 
-void stop()
+void incR()
 {
-  Setpoint = 0.0f;  
+    rightCount++;
 }
 
 void steer()
 {
     float L = _left.read();
     float R = _right.read();
-    if(L == 0.0f && R == 0.0f)
+    if (L == 0.0 && R == 0.0)
     {
-        stop();
-    }    
+        //off track
+        Setpoint = 0.0;
+    }           
     float fb = L - R; 
     float e = SET - fb;
     float Controlleroutput = Kps * e - (Kd * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error?? 
@@ -150,17 +180,9 @@
     if(iTerm < MINM) iTerm = MINM; 
     float output = KPM*error + iTerm;
     if(output > MAXM) output = MAXM;
-    if(output < MINM) output = MINM;    
-    if(output < MINM) 
-    {
-        gateDrive.write(MINM);
-        brake.write(1);
-    }
-    else 
-    {
-        brake.write(0);
-        gateDrive.write(output);
-    }     
+    if(output < MINM) output = MINM;        
+
+    gateDrive.write(output);
     data[4] = gateDrive.read();
     data[5] = speed.read(); 
 }
@@ -174,7 +196,8 @@
 int main()
 {   
     //startup checks
-    bt.attach(&serCb);
+    bt.baud(115200);
+    bt.attach(&serCb);    
     servoSig.period(STEER_FREQ);
     gateDrive.period(.00005f);
     gateDrive.write(Setpoint);
@@ -182,8 +205,12 @@
     ctrlTimer.start();
     control.attach(&cb, TI);
     
-    bt.baud(115200);
-    bt.printf("Right Left SteerControl SteerError GateDrive Speed \r\n");
+    rightCount = 0;
+    leftCount = 0; 
+
+    navRt.fall(&incR);
+    navLft.fall(&incL);
+    
     while(1) {
         myled = !myled;
         //bt.printf("%f ",data[0]);