Diff: main.cpp

Revision:

13:4e77264f254a

Parent:

12:da96e2f87465

Child:

14:13085e161dd1

--- a/main.cpp	Mon Nov 21 17:19:05 2016 +0000
+++ b/main.cpp	Tue Nov 29 13:11:20 2016 +0000
@@ -22,7 +22,6 @@
 
 #define RIGHT_MOTOR_COMPENSATION_RATIO 1.1586276
 
-//Serial pc(USBTX,USBRX);
 Serial pc(PTD3,PTD2);
 XBEE xb(&pc);
 
@@ -38,24 +37,15 @@
 int diff;
 int prev;
 int i = 0;
-float measuredValBuffer[5];
+float measuredValBuffer[64];
 uint8_t valBufferIndex;
 
 //Some PID variables
 pid_instance servo_pid;
+pid_instance right_motor_pid;
+pid_instance left_motor_pid;
 Timer t;
 
-float right_p;
-float right_error;
-float right_perror;
-float right_measured_value, right_desired_value;
-float right_output;
-
-float left_p;
-float left_error;
-float left_perror;
-float left_measured_value, left_desired_value;
-float left_output;
 
 //Speed Sensors variables
 InterruptIn leftHallSensor(D0);
@@ -84,16 +74,18 @@
 inline float findCentreValue();
 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();
 
 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);
-    
+   
     //Setup baud rate for serial link, do not change!
     pc.baud(BAUD_RATE);
     
@@ -101,37 +93,33 @@
     initVariables();
     initSpeedSensors();
               
+
     while(1) {
         
         handleComms();
         
         //If we have an image ready
-        if(TFC_LineScanImageReady>0) {       
+        if(TFC_LineScanImageReady>0) {
+            /* Find the bounds of the track and calculate how close we are to
+             * the centre */        
             servo_pid.measured_value = findCentreValue();
+            recordMeasuredVal();
             
+            // Read the angular velocity of both wheels
+            wL=Get_Speed(Time_L);
+            wR=Get_Speed(Time_R);
+            
+            // Run the PID controllers and adjust steering/motor accordingly
             PIDController();
             
+            // Send the line scan image over serial
             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;      
-                }
-            }
+            // Send the wheel speeds over serial
+            sendSpeeds();
+            
+            // Check if car is at the stop line
+            //handleStartStop();
             
             //Reset image ready flag
             TFC_LineScanImageReady=0;
@@ -139,6 +127,24 @@
     }
 }
 
+void sendBattery() {
+    
+    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;
 
@@ -149,37 +155,68 @@
     pc.putc(0);
 }
 
+void initPID(pid_instance* pid, float Kp, float Ki, float Kd) {
+    pid->Kp = Kd;
+    pid->Ki = Ki;
+    pid->Kd = Kd;
+    pid->dt = 0;
+    pid->p_error = 0;
+    pid->pid_error = 0;
+    pid->integral = 0;
+    pid->measured_value = 0;
+    pid->desired_value = 0;
+    pid->derivative = 0;
+}
+
 void initVariables() {
-    //Tunable PID variables
-    servo_pid.Kp = 125   / 25.0f;
-    servo_pid.Ki = 12.0f / 25.0f;
-    servo_pid.Kd = 0.0f;
-    servo_pid.dt = 0;
-    servo_pid.p_error = 0;
-    servo_pid.pid_error = 0;
-    servo_pid.integral = 0;
-    servo_pid.measured_value = 0;
-    servo_pid.desired_value = 0;
-    servo_pid.derivative = 0;
+    // 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;
+}
+
+int turning = 0;
+int keepTurning = 0;
+bool slow = false;
+inline void recordMeasuredVal() {
     
-    // motor p controller init
-    right_p = 1.0;
-    right_error = 0.0;
-    right_perror = 0.0;
-    right_measured_value = 0.0;
-    right_desired_value = 0.0;
-    right_output = 0.0;
-    left_p = 1.0;
-    left_error = 0.0;
-    left_perror = 0.0;
-    left_measured_value = 0.0;
-    left_desired_value = 0.0;
-    left_output = 0.0;
+    measuredValBuffer[frame_counter % 64] = servo_pid.measured_value;
+    
+    int count = 0;
+    for(i = 0; i < 10; i++) {
+        float val = abs(measuredValBuffer[(frame_counter - i) % 64]);
+        if(val > 0.09) {
+            count++;
+        }
+    } 
     
-    //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)
+    if(abs(servo_pid.measured_value) > 0.11f){
+        if(!turning) {
+            sendString("start turning");
+            
+            TFC_SetMotorPWM(0.2,0.2);
+            turning = 1;
+            
+        } else {
+            turning++;   
+        }
+        
+    } else {
+        if(turning) {
+            if(keepTurning == 0 || count > 6) {
+                keepTurning++;
+            } else {
+                sendString("stop turning turned=%d",turning);
+                keepTurning = 0;
+                turning = 0;    
+                TFC_SetMotorPWM(speed,speed);
+            }
+               
+        }
+    }
+    
 }
 
 inline void sendImage() {
@@ -188,30 +225,33 @@
         pc.putc('H');
         for(i = 0; i < 128; i++) {
             pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);    
-        }
-        
-        wL = wL/3.0f;
-        wR= wR/3.0f;
-        sendString("wL = %f, wR = %f",wL,wR);
-        wL = 0;
-        wR = 0;
-        
-             union {
+        }    
+        sendBattery();
+    }    
+       
+    frame_counter++;  
+}
+
+inline void sendSpeeds() {
+    union {
         float a;
         unsigned char bytes[4];
     } thing;
-    thing.a = TFC_ReadBatteryVoltage();
-    pc.putc('J');        
+    
+    pc.putc('B');
+    thing.a = wL;
     pc.putc(thing.bytes[0]);
     pc.putc(thing.bytes[1]);
     pc.putc(thing.bytes[2]);
     pc.putc(thing.bytes[3]);
-    }
-        wL+=Get_Speed(Time_L);
-        wR+=Get_Speed(Time_R);
-    frame_counter++;
+    thing.a = wR;
+    pc.putc(thing.bytes[0]);
+    pc.putc(thing.bytes[1]);
+    pc.putc(thing.bytes[2]);
+    pc.putc(thing.bytes[3]);    
 }
 
+
 inline void handleComms() {
     if(curr_cmd != 0) {
             switch(curr_cmd) {
@@ -223,9 +263,7 @@
                         servo_pid.Kp = p/25.0f;
                         servo_pid.Ki = i/25.0f;
                         servo_pid.Kd = d/25.0f;
-                        pc.putc('E');
-                        pc.printf("pid change, Kp: %f, Ki: %f, Kd: %f, p: %u, i: %u, d: %u", servo_pid.Kp, servo_pid.Ki, servo_pid.Kd, p, i, d);
-                        pc.putc(0);
+                        sendString("pid= Kp: %f, Ki: %f, Kd: %f, p: %u, i: %u, d: %u", servo_pid.Kp, servo_pid.Ki, servo_pid.Kd, p, i, d);
                         
                         curr_cmd = 0;        
                     }    
@@ -236,15 +274,14 @@
                         char a = xb.cBuffer->read();
                         speed = a/256.0f;
                         TFC_SetMotorPWM(speed,speed);  
-                        pc.putc('E');
-                        pc.printf("s = %u %f",a, speed);
-                        pc.putc(0);
+                        sendString("s = %u %f",a, speed);
                         curr_cmd = 0;   
-                        
                     }
                 break;
                   
                 default:
+                // Unrecognised command
+                curr_cmd = 0;
                 break; 
             }
         }
@@ -270,7 +307,6 @@
         }
 }
 inline float findCentreValue() {
-    float measuredValue;
    
     diff = 0;
     prev = -1;
@@ -298,11 +334,9 @@
     }
     
     //Calculate how left/right we are
-    measuredValue = (64 - ((left+right)/2))/64.f;
-    measuredValBuffer[valBufferIndex % 5] = measuredValue;
-    valBufferIndex++;
+    servo_pid.measured_value = (64 - ((left+right)/2))/64.f;
     
-    return measuredValue;
+    return servo_pid.measured_value;
 }
 
 void handlePID(pid_instance *pid) {
@@ -322,9 +356,15 @@
 }
 
 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))
     {
@@ -332,15 +372,8 @@
     }
     else //Unhappy PID state
     {        
-        pc.putc('E');
-        pc.printf("pid unhappy");
-        pc.putc(0);
-        pc.putc('E');
-        pc.printf("out = %f p_err = %f", servo_pid.output, servo_pid.p_error);
-        pc.putc(0);
+        sendString("out = %f p_err = %f", servo_pid.output, servo_pid.p_error);
         TFC_InitServos(0.00052, 0.00122, 0.02);
-        //output, pid_error, p_error, integral, derivative = 0;
-        
         if(servo_pid.output >= 1.0f) {
             TFC_SetServo(0, 0.9f);
             servo_pid.output = 1.0f;
@@ -348,27 +381,37 @@
             TFC_SetServo(0, -0.9f);
             servo_pid.output = -1.0f;
         }
-    }
-   
-    /*
-    right_error = right_desired_value - right_measured_value;
-    right_output = right_p * right_error;
-    right_perror = right_error;
+    }    
     
-    left_error = left_desired_value - left_measured_value;
-    left_output = left_p * left_error;
-    left_perror = left_error;
-    
-    TFC_SetMotorPWM(right_output,left_output);
-    
-    dutyCycleCorner(speed,output);
-    */
     
     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);
+            wait(0.08);
+            TFC_SetMotorPWM(-0.6f,-0.6f);
+            wait(0.3);
+            TFC_SetMotorPWM(0.f,0.f);
+            TFC_HBRIDGE_DISABLE;
+            startstop = 0;      
+        }
+    }    
+}
 
 
 inline void initSpeedSensors() {