Diff: main.cpp

Revision:

16:81cdffd8c5d5

Parent:

15:ccde02f96449

Child:

17:6ae90788cc2b

--- a/main.cpp	Thu Dec 01 16:56:17 2016 +0000
+++ b/main.cpp	Fri Dec 02 11:06:17 2016 +0000
@@ -33,8 +33,8 @@
 uint8_t curr_right;
 uint8_t right;
 uint8_t left;
-int8_t leftChange;
-int8_t rightChange;
+uint8_t farRight;
+uint8_t farLeft;
 int diff;
 int prev;
 int i = 0;
@@ -43,6 +43,8 @@
 
 //Communication Variables
 uint8_t sendCam = 0;
+int frame_counter = 0;
+char curr_cmd = 0; // Current comms command
 
 //Some PID variables
 pid_instance servo_pid;
@@ -62,11 +64,10 @@
 void GetTime_R();
 inline void initSpeedSensors();
 
-// Current comms command
-char curr_cmd = 0;
+
 
 float speed = 0.3;
-int frame_counter = 0;
+
 
 //Hacky start/stop signal detection
 int startstop = 0;
@@ -75,7 +76,7 @@
 void sendString(const char *format, ...);
 void initVariables();
 inline void handleComms();
-inline float findCentreValue(volatile uint16_t * cam_data);
+inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &left, uint8_t &right);
 inline void PIDController();
 inline void sendImage();
 inline void sendSpeeds();
@@ -108,7 +109,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(TFC_LineScanImage0);
+            servo_pid.measured_value = findCentreValue(TFC_LineScanImage0, left, right);
             recordMeasuredVal();
             
             // Read the angular velocity of both wheels
@@ -189,7 +190,7 @@
 bool slow = false;
 inline void recordMeasuredVal() {
     
-    float aheadForward = findCentreValue(TFC_LineScanImage1);
+    float aheadForward = findCentreValue(TFC_LineScanImage1, farLeft, farRight);
     
     measuredValBuffer[frame_counter % 64] = servo_pid.measured_value;
     
@@ -355,37 +356,36 @@
         }
 }
 
-inline float findCentreValue(volatile uint16_t * cam_data) {
+inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &l, uint8_t &r) {
    
     diff = 0;
     prev = -1;
-    leftChange = left;
     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) {
-            left = i;
+            l = i;
             break;
         }
         prev = curr_left;
     }
     
     prev = -1;
-    rightChange = right;
     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) {
-            right = i;
+            r = i;
             break;
         }
         prev = curr_right;
     }
     
     //Calculate how left/right we are   
-    return (64 - ((left+right)/2))/64.f;
+    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;