Diff: main.cpp

Revision:

8:7c5e6b1e7aa5

Parent:

7:ad893fc41b95

Child:

9:aa2ce38dec6b

--- a/main.cpp	Mon Nov 14 11:11:13 2016 +0000
+++ b/main.cpp	Mon Nov 14 13:42:22 2016 +0000
@@ -1,20 +1,51 @@
+//Autonomous Car GDP controller
+//Written by various group members
+//Commented & cleaned up by Max/Adam
+
+//To-do
+// -Change xbee transmission to non-blocking
+// -Improve start/stop detection and resultant action (setting PID values?)
+
+#include <stdarg.h>
+#include <stdio.h>
+
 #include "mbed.h"
 #include "TFC.h"
 #include "XBEE.h"
+
+#define BAUD_RATE 57600
 #define CAM_THRESHOLD 109
- 
- 
-DigitalOut myled(LED1);
+#define CAM_DIFF 10
+
 //Serial pc(USBTX,USBRX);
 Serial pc(PTD3,PTD2);
 XBEE xb(&pc);
 
+//Woo global variables!
+bool onTrack;
 char curr_line[128];
 uint8_t curr_left;
 uint8_t curr_right;
-
 uint8_t right;
 uint8_t left;
+int8_t leftChange;
+int8_t rightChange;
+int diff;
+int prev;
+int i = 0;
+float measuredValBuffer[5];
+uint8_t valBufferIndex;
+
+//Some PID variables
+float Kp;
+float Ki;
+float Kd;
+float dt;
+float p_error;
+float pid_error;
+float integral;
+float measured_value, desired_value, derivative;
+float output;
 
 Timer t;
 
@@ -22,43 +53,112 @@
 
 float speed = 0.3;
 int frame_counter = 0;
- 
- int startstop = 0;
+
+//Hacky start/stop signal detection
+int startstop = 0;
 bool seen = false;
- 
+
+void sendString(const char *format, ...);
+void initVariables();
+inline void handleComms();
+inline float findCentreValue();
+inline void PIDController();
+inline void sendImage();
+
 int main() {
+    //Set up TFC driver stuff
     TFC_Init();
     TFC_InitServos(0.00052,0.00122,0.02);
+    //Old things to make the car move at run-time
+    //Should tie these to a button for the actual races
     //TFC_HBRIDGE_ENABLE;
     //TFC_SetMotorPWM(0.3,0.3);
     
-    
-    pc.baud(57600);
+    //Setup baud rate for serial link, do not change!
+    pc.baud(BAUD_RATE);
     
-    float p_error, error;
-    float integral;
-    float measured_value, desired_value,derivative;
-    float output;
-      //tunable variables
-    float Kp, Ki,Kd;
-        Kp=125/25.0f;
-    Ki=12.0f/25.0f;
-    Kd=0.0;
-    myled = 0;// Test
-        float dt=0;
-    p_error=0;
-    error=0;
-    integral=0;
-    measured_value= 0;
-    desired_value=0;
-    derivative=0;
-        // measured value is a float between -1.0 and 1.0
-    // desired value is always 0 ( as in car is in the middle of the road)
+    //Initialise/reset PID variables
+    initVariables();
     
-    uint8_t i = 0;
     while(1) {
         
-        if(curr_cmd != 0) {
+        handleComms();
+        
+        //If we have an image ready
+        if(TFC_LineScanImageReady>0) {       
+            measured_value = findCentreValue();
+            
+            PIDController();
+            
+            sendImage();
+            
+            //Hacky way to detect the start/stop signal
+            if(right - left < 60) {
+                pc.putc('E');
+                pc.printf("START STOP!! &d",startstop);
+                pc.putc(0);            
+                
+                if(seen) {
+                    seen = false;
+                } else {
+                    startstop++;
+                    seen = true;    
+                }    
+                //If we've done 5 laps, stop the car
+                if(startstop >= 1) {
+                    TFC_SetMotorPWM(0.0,0.0);
+                    TFC_HBRIDGE_DISABLE;
+                    startstop = 0;      
+                }
+            }
+            
+            //Reset image ready flag
+            TFC_LineScanImageReady=0;
+        }
+    }
+}
+
+void sendString(const char *format, ...) {
+    va_list arg;
+
+    pc.putc('E');
+    va_start (arg, format); 
+    pc.vprintf(format,arg);
+    va_end (arg);
+    pc.putc(0);
+}
+
+void initVariables() {
+    //Tunable PID variables
+    Kp = 125   / 25.0f;
+    Ki = 12.0f / 25.0f;
+    Kd = 0.0f;
+    dt = 0;
+    p_error = 0;
+    pid_error = 0;
+    integral = 0;
+    measured_value = 0;
+    desired_value = 0;
+    derivative = 0;
+    
+    valBufferIndex = 0;
+    //Measured value is a float between -1.0 and 1.0 (from left to right)
+    //Desired value is always 0.0 (as in, car is in the middle of the road)
+}
+
+inline void sendImage() {
+     //Only send 1/3 of camera frames to GUI program
+    if((frame_counter % 3) == 0) {
+        pc.putc('H');
+        for(i = 0; i < 128; i++) {
+            pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);    
+        }
+    }
+    frame_counter++;
+}
+
+inline void handleComms() {
+    if(curr_cmd != 0) {
             switch(curr_cmd) {
                 case 'A':
                     if(xb.cBuffer->available() >= 3) {
@@ -69,8 +169,9 @@
                         Ki = i/25.0f;
                         Kd = d/25.0f;
                         pc.putc('E');
-                        pc.printf("pid change");
+                        pc.printf("pid change, Kp: %f, Ki: %f, Kd: %f, p: %u, i: %u, d: %u", Kp, Ki, Kd, p, i, d);
                         pc.putc(0);
+                        
                         curr_cmd = 0;        
                     }    
                 break;
@@ -111,135 +212,85 @@
             }
             
         }
-        
-        //If we have an image ready
-        if(TFC_LineScanImageReady>0) {       
-            //left = 0;
-            //right = 0;
-            int diff = 0;
-            int prev = -1;
-            for(i = 63; i > 0; i--) {
-                curr_left = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
-               /* if(curr_left < CAM_THRESHOLD) {
-                    left = i;
-                    break;
-                }
-                */
-                
-                diff = prev - curr_left;
-                if(abs(diff) >= 10 && curr_left <= 100 && prev != -1) {
-                    left = i;
-                    break;    
-                }
-                
-                prev = curr_left;
-            }
-            
-            prev = -1;
-            for(i = 64; i < 128; i++) {
-                curr_right = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
-                /*
-                if(curr_right < CAM_THRESHOLD) {
-                    right = i;
-                    break;
-                }
-                */
-                
-                int diff = prev - curr_right;
-                if(abs(diff) >= 10 && curr_right <= 100 && prev != -1) {
-                    right = i;
-                    break;    
-                }
-                
-                prev = curr_right;
-            }
+}
+inline float findCentreValue() {
+    float measuredValue;
+   
+    diff = 0;
+    prev = -1;
+    leftChange = left;
+    for(i = 63; i > 0; i--) {
+        curr_left = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;              
+        diff = prev - curr_left;
+        if(abs(diff) >= 10 && curr_left <= 100 && prev != -1) {
+            left = i;
+            break;
+        }
+        prev = curr_left;
+    }
+    
+    prev = -1;
+    rightChange = right;
+    for(i = 64; i < 128; i++) {
+        curr_right = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
+        int diff = prev - curr_right;
+        if(abs(diff) >= 10 && curr_right <= 100 && prev != -1) {
+            right = i;
+            break;
+        }
+        prev = curr_right;
+    }
+    
+    //Calculate how left/right we are
+    measuredValue = (64 - ((left+right)/2))/64.f;
+    measuredValBuffer[valBufferIndex % 5] = measuredValue;
+    valBufferIndex++;
+    
+    return measuredValue;
+}
 
-            if((frame_counter % 3) == 0) {
-            pc.putc('H');
-            for(i = 0; i < 128; i++) {
-                pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);    
-            }
-            }
-            frame_counter++;
-            
-            measured_value = (64 - ((left+right)/2))/64.f;
-            
-            if(right - left < 60) {
-                pc.putc('E');
-                pc.printf("START STOP!! &d",startstop);
-                pc.putc(0);            
-                
-                if(seen) {
-                    seen = false;
-                } else {
-                    startstop++;
-                    seen = true;    
-                }    
-            
-            if(startstop >= 5) {
-                TFC_SetMotorPWM(0.0,0.0);
-                TFC_HBRIDGE_DISABLE;
-                startstop = 0;      
-            }
-            
-            } 
-            
+inline void PIDController() {
+    //PID Stuff!
     t.start();
-    dt=t.read();
-    error = desired_value - measured_value;
-   integral=integral+error*dt;
-   derivative=(error-p_error)/dt;
-   output=Kp*error+Ki*integral+Kd*derivative;
-   p_error=error;
-   
-   
-   if(integral > 1.0f) {
+    dt = t.read();
+    pid_error = desired_value - measured_value;
+    integral = integral + pid_error * dt;
+    derivative = (pid_error - p_error) / dt;
+    output = Kp * pid_error + Ki * integral + Kd * derivative;
+    p_error = pid_error;
+    
+    if(integral > 1.0f) {
         integral = 1.0f;   
     }
     if(integral < -1.0f) {
         integral = -1.0f;   
     }
-   
     
-   if((-1.0<=output)&&(output<=1.0))
-   {
-      TFC_SetServo(0,output);
- 
-    }
-    else
+    if((-1.0 <= output) && (output <= 1.0))
     {
-          
-            pc.putc('E');
-    pc.printf("pid unhappy\0");
-    pc.putc('E');
-    pc.printf("out = %f p_err = %f\0",output, p_error);
-    TFC_InitServos(0.00052,0.00122,0.02);
-    //output, error, p_error, integral, derivative = 0;
+        TFC_SetServo(0, output);
+    }
+    else //Unhappy PID state
+    {        
+        pc.putc('E');
+        pc.printf("pid unhappy");
+        pc.putc(0);
+        pc.putc('E');
+        pc.printf("out = %f p_err = %f", output, p_error);
+        pc.putc(0);
+        TFC_InitServos(0.00052, 0.00122, 0.02);
+        //output, pid_error, p_error, integral, derivative = 0;
+        
+        if(output >= 1.0f) {
+            TFC_SetServo(0, 0.9f);
+            output = 1.0f;
+        } else {
+            TFC_SetServo(0, -0.9f);
+            output = -1.0f;
+        }
+    }
     
-    if(output >= 1.0f) {
-        TFC_SetServo(0,0.9f);
-        output = 1.0f;
-    } else {
-        TFC_SetServo(0,-0.9f);
-        output = -1.0f;
-    } 
-    
-    
-  
-    
-    
-   }
-       
     t.stop();
     t.reset();
     t.start();
- 
-            
-        
-
-            //Reset image ready flag
-            TFC_LineScanImageReady=0;
-            wait(0.02f);
-        }
-    }
 }
\ No newline at end of file