Diff: main.cpp

Revision:

17:6ae90788cc2b

Parent:

16:81cdffd8c5d5

Child:

18:0095a3a8f8e4

--- a/main.cpp	Fri Dec 02 11:06:17 2016 +0000
+++ b/main.cpp	Fri Dec 02 14:36:37 2016 +0000
@@ -16,85 +16,31 @@
 #include "main.h"
 #include "motor.h"
 
-#define BAUD_RATE 57600
-#define CAM_THRESHOLD 109
-#define CAM_DIFF 10
-#define WHEEL_RADIUS 0.025f
-
-#define RIGHT_MOTOR_COMPENSATION_RATIO 1.1586276
-
-Serial pc(PTD3,PTD2);
-XBEE xb(&pc);
+// Serial
+#if USE_COMMS
+    Serial pc(PTD3,PTD2);
+    XBEE xb(&pc);
+#endif
 
-//Woo global variables!
-bool onTrack;
-char curr_line[128];
-uint8_t curr_left;
-uint8_t curr_right;
-uint8_t right;
-uint8_t left;
-uint8_t farRight;
-uint8_t farLeft;
-int diff;
-int prev;
-int i = 0;
-float measuredValBuffer[64];
-uint8_t valBufferIndex;
-
-//Communication Variables
-uint8_t sendCam = 0;
-int frame_counter = 0;
-char curr_cmd = 0; // Current comms command
-
-//Some PID variables
-pid_instance servo_pid;
-pid_instance right_motor_pid;
-pid_instance left_motor_pid;
+// PID Timer
 Timer t;
 
-
-//Speed Sensors variables
+//Speed Sensors interupts and timers
 InterruptIn leftHallSensor(D0);
 InterruptIn rightHallSensor(D2);
 Timer t1;
 Timer t2;
-volatile float Time_L,Time_R;
-float wL, wR;
-void GetTime_L();
-void GetTime_R();
-inline void initSpeedSensors();
 
 
-
-float speed = 0.3;
-
-
-//Hacky start/stop signal detection
-int startstop = 0;
-bool seen = false;
-
-void sendString(const char *format, ...);
-void initVariables();
-inline void handleComms();
-inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &left, uint8_t &right);
-inline void PIDController();
-inline void sendImage();
-inline void sendSpeeds();
-void handlePID(pid_instance *pid);
-void initPID(pid_instance* pid);
-inline void handleStartStop();
-inline void recordMeasuredVal();
-void sendBattery();
-inline float getLineEntropy();
- 
-
 int main() {
     //Set up TFC driver stuff
     TFC_Init();
-    TFC_InitServos(0.00052,0.00122,0.02);
+    ALIGN_SERVO;
    
-    //Setup baud rate for serial link, do not change!
-    pc.baud(BAUD_RATE);
+    #if USE_COMMS
+        //Setup baud rate for serial link, do not change!
+        pc.baud(BAUD_RATE);
+    #endif
     
     //Initialise/reset PID variables
     initVariables();
@@ -103,27 +49,33 @@
 
     while(1) {
         
-        handleComms();
+        #if USE_COMMS
+            handleComms();
+        #endif
         
         //If we have an image ready
         if(TFC_LineScanImageReady>0) {
             /* Find the bounds of the track and calculate how close we are to
              * the centre */        
-            servo_pid.measured_value = findCentreValue(TFC_LineScanImage0, left, right);
-            recordMeasuredVal();
+            servo_pid.measured_value = findCentreValue(CLOSE_CAMERA, left, right);
             
             // Read the angular velocity of both wheels
             wL=Get_Speed(Time_L);
             wR=Get_Speed(Time_R);
             
+            // Slow down, adjust PID values and enable differential before corners.
+            handleCornering();
+            
             // Run the PID controllers and adjust steering/motor accordingly
             PIDController();
             
-            // Send the line scan image over serial
-            sendImage();
-            
-            // Send the wheel speeds over serial
-            sendSpeeds();
+            #if USE_COMMS
+                // Send the line scan image over serial
+                sendImage();
+                
+                // Send the wheel speeds over serial
+                sendSpeeds();
+            #endif
             
             // Check if car is at the stop line
             //handleStartStop();
@@ -135,36 +87,27 @@
     }
 }
 
-void sendBattery() {
+void initVariables() {
+    // Initialise three PID controllers for the servo and each wheel.
+    initPID(&servo_pid, 2.2f, 0.6f, 0.f);
+    initPID(&left_motor_pid, 1.0f, 0.f, 0.f);
+    initPID(&right_motor_pid, 1.0f, 0.f, 0.f);
+    
+    valBufferIndex = 0;
+    speed = 0.3;
     
-    if(frame_counter % 256 == 0) {
-        float level = TFC_ReadBatteryVoltage() * 6.25;
-         union {
-            float a;
-            unsigned char bytes[4];
-        } thing;
-        
-        pc.putc('J');
-        thing.a = level;
-        pc.putc(thing.bytes[0]);
-        pc.putc(thing.bytes[1]);
-        pc.putc(thing.bytes[2]);
-        pc.putc(thing.bytes[3]);
-    }
-}
-
-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);
+    //Start stop
+    startstop = 0;
+    seen = false;
+    
+    // Turning
+    turning = 0;
+    keepTurning = 0;
+    slow = false;
 }
 
 void initPID(pid_instance* pid, float Kp, float Ki, float Kd) {
-    pid->Kp = Kd;
+    pid->Kp = Kp;
     pid->Ki = Ki;
     pid->Kd = Kd;
     pid->dt = 0;
@@ -176,21 +119,38 @@
     pid->derivative = 0;
 }
 
-void initVariables() {
-    // Initialise three PID controllers for the servo and each wheel.
-    initPID(&servo_pid, 2.2f, 0.6f, 0.f);
-    initPID(&left_motor_pid, 1.0f, 0.f, 0.f);
-    initPID(&right_motor_pid, 1.0f, 0.f, 0.f);
+inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &l, uint8_t &r) {
+   
+    diff = 0;
+    prev = -1;
+    for(i = 63; i > 0; i--) {
+        curr_left = (int8_t)(cam_data[i] >> 4) & 0xFF;              
+        diff = prev - curr_left;
+        if(abs(diff) >= CAM_DIFF && curr_left <= CAM_THRESHOLD && prev != -1) {
+            l = i;
+            break;
+        }
+        prev = curr_left;
+    }
     
-    valBufferIndex = 0;
+    prev = -1;
+    for(i = 64; i < 128; i++) {
+        curr_right = (int8_t)(cam_data[i] >> 4) & 0xFF;
+        int diff = prev - curr_right;
+        if(abs(diff) >= CAM_DIFF && curr_right <= CAM_THRESHOLD && prev != -1) {
+            r = i;
+            break;
+        }
+        prev = curr_right;
+    }
+    
+    //Calculate how left/right we are   
+    return (64 - ((l+r)/2))/64.f;
 }
 
-int turning = 0;
-int keepTurning = 0;
-bool slow = false;
-inline void recordMeasuredVal() {
+inline void handleCornering() {
     
-    float aheadForward = findCentreValue(TFC_LineScanImage1, farLeft, farRight);
+    float lookaheadMeasuredValue = findCentreValue(LOOKAHEAD_CAMERA, farLeft, farRight);
     
     measuredValBuffer[frame_counter % 64] = servo_pid.measured_value;
     
@@ -202,7 +162,7 @@
         }
     } 
     
-    if(!turning && abs(aheadForward) > 0.11f){
+    if(!turning && abs(lookaheadMeasuredValue) > 0.11f){
         TFC_SetMotorPWM(0.4,0.4);    
     }
     
@@ -234,17 +194,140 @@
     
 }
 
+inline float getLineEntropy() {
+    float entropy = 0;
+    float last = (int8_t)(CLOSE_CAMERA[0] >> 4) & 0xFF;
+    for(int i = 1; i < 128; i++) {
+        entropy += abs(last - ((int8_t)(CLOSE_CAMERA[i] >> 4) & 0xFF));
+    }
+    return entropy;
+}
+
+void handlePID(pid_instance *pid) {
+    pid->dt = t.read();
+    pid->pid_error = pid->desired_value - pid->measured_value;
+    pid->integral = pid->integral + pid->pid_error * pid->dt;
+    pid->derivative = (pid->pid_error - pid->p_error) / pid->dt;
+    pid->output = pid->Kp * pid->pid_error + pid->Ki * pid->integral + pid->Kd * pid->derivative;
+    pid->p_error = pid->pid_error;
+    
+    if(pid->integral > 1.0f) {
+        pid->integral = 1.0f;   
+    }
+    if(pid->integral < -1.0f) {
+        pid->integral = -1.0f;   
+    }
+}
+
+inline void PIDController() {
+    // update motor measurements
+    left_motor_pid.measured_value = wL;
+    right_motor_pid.measured_value = wR;
+    
+    //PID Stuff!
+    t.start();
+    handlePID(&servo_pid);
+    handlePID(&left_motor_pid);
+    handlePID(&right_motor_pid);
+
+    if((-1.0 <= servo_pid.output) && (servo_pid.output <= 1.0))
+    {
+        TFC_SetServo(0, servo_pid.output);
+    }
+    else //Unhappy PID state
+    {        
+        //sendString("out = %f p_err = %f", servo_pid.output, servo_pid.p_error);
+        ALIGN_SERVO;
+        if(servo_pid.output >= 1.0f) {
+            TFC_SetServo(0, 0.9f);
+            servo_pid.output = 1.0f;
+        } else {
+            TFC_SetServo(0, -0.9f);
+            servo_pid.output = -1.0f;
+        }
+    }    
+    
+    
+    t.stop();
+    t.reset();
+    t.start();
+}
+
+inline void handleStartStop() {
+    //Hacky way to detect the start/stop signal
+    if(right - left < 60) {
+        sendString("START STOP!! &d",startstop); 
+        
+        if(seen) {
+            seen = false;
+        } else {
+            startstop++;
+            seen = true;    
+        }    
+        //If we've done 5 laps, stop the car
+        if(startstop >= 1) {
+            TFC_SetMotorPWM(0.f,0.f);
+            TFC_HBRIDGE_DISABLE;
+            startstop = 0;      
+        }
+    }    
+}
+
+
+inline void initSpeedSensors() {
+    t1.start();
+    t2.start();
+    
+    //Left and Right are defined looking at the rear of the car, in the direction the camera points at.
+    leftHallSensor.rise(&GetTime_L);
+    rightHallSensor.rise(&GetTime_R);  
+}
+
+void GetTime_L(){
+    Time_L=t1.read_us();
+    t1.reset();
+}
+
+void GetTime_R(){
+    Time_R=t2.read_us();
+    t2.reset();
+}
+
+#if USE_COMMS
+void sendBattery() {
+    
+    if(frame_counter % 256 == 0) {
+        float level = TFC_ReadBatteryVoltage() * 6.25;          
+        pc.putc('J');
+        thing.a = level;
+        pc.putc(thing.bytes[0]);
+        pc.putc(thing.bytes[1]);
+        pc.putc(thing.bytes[2]);
+        pc.putc(thing.bytes[3]);
+    }
+}
+
+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);
+}
+
 inline void sendImage() {
      //Only send 1/3 of camera frames to GUI program
     if((frame_counter % 3) == 0) {
         pc.putc('H');
         if(sendCam == 0) {            
             for(i = 0; i < 128; i++) {
-                pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);    
+                pc.putc((int8_t)(CLOSE_CAMERA[i] >> 4) & 0xFF);    
             }    
         } else {
             for(i = 0; i < 128; i++) {
-                pc.putc((int8_t)(TFC_LineScanImage1[i] >> 4) & 0xFF);    
+                pc.putc((int8_t)(LOOKAHEAD_CAMERA[i] >> 4) & 0xFF);    
             } 
         }
         sendBattery();
@@ -253,21 +336,8 @@
     frame_counter++;  
 }
 
-inline float getLineEntropy() {
-    float entropy = 0;
-    float last = (int8_t)(TFC_LineScanImage0[0] >> 4) & 0xFF;
-    for(int i = 1; i < 128; i++) {
-        entropy += abs(last - ((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF));
-    }
-    return entropy;
-}
-
 inline void sendSpeeds() {
-    union {
-        float a;
-        unsigned char bytes[4];
-    } thing;
-    
+  
     float en = getLineEntropy();
     
     if(onTrack) {
@@ -336,7 +406,7 @@
         if(xb.cBuffer->available() > 0 && curr_cmd == 0) {
             char cmd = xb.cBuffer->read();
             if(cmd == 'D') {
-                TFC_InitServos(0.00052,0.00122,0.02);
+                ALIGN_SERVO;
                 TFC_HBRIDGE_ENABLE;
                 TFC_SetMotorPWM(RIGHT_MOTOR_COMPENSATION_RATIO*speed,speed);
                 servo_pid.integral = 0;
@@ -355,123 +425,4 @@
             
         }
 }
-
-inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &l, uint8_t &r) {
-   
-    diff = 0;
-    prev = -1;
-    for(i = 63; i > 0; i--) {
-        curr_left = (int8_t)(cam_data[i] >> 4) & 0xFF;              
-        diff = prev - curr_left;
-        if(abs(diff) >= 10 && curr_left <= 100 && prev != -1) {
-            l = i;
-            break;
-        }
-        prev = curr_left;
-    }
-    
-    prev = -1;
-    for(i = 64; i < 128; i++) {
-        curr_right = (int8_t)(cam_data[i] >> 4) & 0xFF;
-        int diff = prev - curr_right;
-        if(abs(diff) >= 10 && curr_right <= 100 && prev != -1) {
-            r = i;
-            break;
-        }
-        prev = curr_right;
-    }
-    
-    //Calculate how left/right we are   
-    return (64 - ((l+r)/2))/64.f;
-}
-
-
-void handlePID(pid_instance *pid) {
-    pid->dt = t.read();
-    pid->pid_error = pid->desired_value - pid->measured_value;
-    pid->integral = pid->integral + pid->pid_error * pid->dt;
-    pid->derivative = (pid->pid_error - pid->p_error) / pid->dt;
-    pid->output = pid->Kp * pid->pid_error + pid->Ki * pid->integral + pid->Kd * pid->derivative;
-    pid->p_error = pid->pid_error;
-    
-    if(pid->integral > 1.0f) {
-        pid->integral = 1.0f;   
-    }
-    if(pid->integral < -1.0f) {
-        pid->integral = -1.0f;   
-    }
-}
-
-inline void PIDController() {
-    // update motor measurements
-    left_motor_pid.measured_value = wL;
-    right_motor_pid.measured_value = wR;
-    
-    //PID Stuff!
-    t.start();
-    handlePID(&servo_pid);
-    handlePID(&left_motor_pid);
-    handlePID(&right_motor_pid);
-
-    if((-1.0 <= servo_pid.output) && (servo_pid.output <= 1.0))
-    {
-        TFC_SetServo(0, servo_pid.output);
-    }
-    else //Unhappy PID state
-    {        
-        //sendString("out = %f p_err = %f", servo_pid.output, servo_pid.p_error);
-        TFC_InitServos(0.00052, 0.00122, 0.02);
-        if(servo_pid.output >= 1.0f) {
-            TFC_SetServo(0, 0.9f);
-            servo_pid.output = 1.0f;
-        } else {
-            TFC_SetServo(0, -0.9f);
-            servo_pid.output = -1.0f;
-        }
-    }    
-    
-    
-    t.stop();
-    t.reset();
-    t.start();
-}
-
-inline void handleStartStop() {
-    //Hacky way to detect the start/stop signal
-    if(right - left < 60) {
-        sendString("START STOP!! &d",startstop); 
-        
-        if(seen) {
-            seen = false;
-        } else {
-            startstop++;
-            seen = true;    
-        }    
-        //If we've done 5 laps, stop the car
-        if(startstop >= 1) {
-            TFC_SetMotorPWM(0.f,0.f);
-            TFC_HBRIDGE_DISABLE;
-            startstop = 0;      
-        }
-    }    
-}
-
-
-inline void initSpeedSensors() {
-    t1.start();
-    t2.start();
-    
-    //Left and Right are defined looking at the rear of the car, in the direction the camera points at.
-    leftHallSensor.rise(&GetTime_L);
-    rightHallSensor.rise(&GetTime_R);  
-}
-
-void GetTime_L(){
-    Time_L=t1.read_us();
-    t1.reset();
-}
-
-void GetTime_R(){
-    Time_R=t2.read_us();
-    t2.reset();
-}
\ No newline at end of file
+#endif
\ No newline at end of file