Diff: main.cpp

Revision:

15:ccde02f96449

Parent:

14:13085e161dd1

Child:

16:81cdffd8c5d5

diff -r 13085e161dd1 -r ccde02f96449 main.cpp
--- a/main.cpp	Wed Nov 30 14:54:19 2016 +0000
+++ b/main.cpp	Thu Dec 01 16:56:17 2016 +0000
@@ -41,6 +41,9 @@
 float measuredValBuffer[64];
 uint8_t valBufferIndex;
 
+//Communication Variables
+uint8_t sendCam = 0;
+
 //Some PID variables
 pid_instance servo_pid;
 pid_instance right_motor_pid;
@@ -72,7 +75,7 @@
 void sendString(const char *format, ...);
 void initVariables();
 inline void handleComms();
-inline float findCentreValue();
+inline float findCentreValue(volatile uint16_t * cam_data);
 inline void PIDController();
 inline void sendImage();
 inline void sendSpeeds();
@@ -81,6 +84,8 @@
 inline void handleStartStop();
 inline void recordMeasuredVal();
 void sendBattery();
+inline float getLineEntropy();
+ 
 
 int main() {
     //Set up TFC driver stuff
@@ -103,7 +108,7 @@
         if(TFC_LineScanImageReady>0) {
             /* Find the bounds of the track and calculate how close we are to
              * the centre */        
-            servo_pid.measured_value = findCentreValue();
+            servo_pid.measured_value = findCentreValue(TFC_LineScanImage0);
             recordMeasuredVal();
             
             // Read the angular velocity of both wheels
@@ -122,6 +127,7 @@
             // Check if car is at the stop line
             //handleStartStop();
             
+            
             //Reset image ready flag
             TFC_LineScanImageReady=0;
         }
@@ -183,6 +189,8 @@
 bool slow = false;
 inline void recordMeasuredVal() {
     
+    float aheadForward = findCentreValue(TFC_LineScanImage1);
+    
     measuredValBuffer[frame_counter % 64] = servo_pid.measured_value;
     
     int count = 0;
@@ -193,19 +201,18 @@
         }
     } 
     
+    if(!turning && abs(aheadForward) > 0.11f){
+        TFC_SetMotorPWM(0.4,0.4);    
+    }
+    
     if(turning) {
-        dutyCycleCorner(0.3,servo_pid.output);
+        dutyCycleCorner(0.4,servo_pid.output);
         //sensorCorner(left_motor_pid.desired_value, right_motor_pid.desired_value , servo_pid.output, 50);  
     }
     
     if(abs(servo_pid.measured_value) > 0.11f){
-        if(!turning) {
-            sendString("start turning");
-            
-            TFC_SetMotorPWM(0.2,0.2);
-            
+        if(!turning) {            
             turning = 1;
-            
         } else {
             turning++;   
         }
@@ -215,7 +222,7 @@
             if(keepTurning == 0 || count > 6) {
                 keepTurning++;
             } else {
-                sendString("stop turning turned=%d",turning);
+                //sendString("stop turning turned=%d",turning);
                 keepTurning = 0;
                 turning = 0;    
                 TFC_SetMotorPWM(speed,speed);
@@ -230,29 +237,60 @@
      //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);    
-        }    
+        if(sendCam == 0) {            
+            for(i = 0; i < 128; i++) {
+                pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);    
+            }    
+        } else {
+            for(i = 0; i < 128; i++) {
+                pc.putc((int8_t)(TFC_LineScanImage1[i] >> 4) & 0xFF);    
+            } 
+        }
         sendBattery();
     }    
        
     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) {
+        if(en <= 14000) {
+            onTrack = false;
+            sendString("offfffffffffffff");    
+            TFC_SetMotorPWM(0.0,0.0);
+            TFC_HBRIDGE_DISABLE;
+        }
+    } else {
+        if(en > 14000) {
+            onTrack = true;
+            sendString("ON TRACK");
+        }    
+    }
+    
 
     pc.putc('B');
-    thing.a = wL * WHEEL_RADIUS;
+    thing.a = en;//wL * WHEEL_RADIUS;
     pc.putc(thing.bytes[0]);
     pc.putc(thing.bytes[1]);
     pc.putc(thing.bytes[2]);
     pc.putc(thing.bytes[3]);
-    thing.a = wR * WHEEL_RADIUS;
+    thing.a = en; //wR * WHEEL_RADIUS;
     pc.putc(thing.bytes[0]);
     pc.putc(thing.bytes[1]);
     pc.putc(thing.bytes[2]);
@@ -310,17 +348,20 @@
                 curr_cmd = 'A';
             } else if(cmd == 'F') {
                 curr_cmd = 'F';    
+            } else if(cmd == 'K') {
+                sendCam = ~sendCam;    
             }
             
         }
 }
-inline float findCentreValue() {
+
+inline float findCentreValue(volatile uint16_t * cam_data) {
    
     diff = 0;
     prev = -1;
     leftChange = left;
     for(i = 63; i > 0; i--) {
-        curr_left = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;              
+        curr_left = (int8_t)(cam_data[i] >> 4) & 0xFF;              
         diff = prev - curr_left;
         if(abs(diff) >= 10 && curr_left <= 100 && prev != -1) {
             left = i;
@@ -332,7 +373,7 @@
     prev = -1;
     rightChange = right;
     for(i = 64; i < 128; i++) {
-        curr_right = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
+        curr_right = (int8_t)(cam_data[i] >> 4) & 0xFF;
         int diff = prev - curr_right;
         if(abs(diff) >= 10 && curr_right <= 100 && prev != -1) {
             right = i;
@@ -341,10 +382,8 @@
         prev = curr_right;
     }
     
-    //Calculate how left/right we are
-    servo_pid.measured_value = (64 - ((left+right)/2))/64.f;
-    
-    return servo_pid.measured_value;
+    //Calculate how left/right we are   
+    return (64 - ((left+right)/2))/64.f;
 }
 
 void handlePID(pid_instance *pid) {
@@ -380,7 +419,7 @@
     }
     else //Unhappy PID state
     {        
-        sendString("out = %f p_err = %f", servo_pid.output, servo_pid.p_error);
+        //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);