Diff: main.cpp

Revision:

31:1a06c9e1985e

Parent:

29:b5b31256572b

Child:

32:6829684f8c4d

--- a/main.cpp	Tue Jan 10 11:24:13 2017 +0000
+++ b/main.cpp	Wed Jan 11 11:36:56 2017 +0000
@@ -121,7 +121,6 @@
     wR = 0;
     prevL = 0;
     prevR = 0;
- 
 }
 
 void initPID(pid_instance* pid, float Kp, float Ki, float Kd) {
@@ -139,29 +138,35 @@
 
     bool leftSeen;
     bool rightSeen;
-    
+
+//Function which calcuates how far to the left/right of the centre of the track the car is
+//Takes data from either camera, and passes some variables by reference, which will hold
+//the indices holding the locations of the left and right edges of the track
 inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &l, uint8_t &r) {
    
-    diff = 0;
-    prev = -1;
+    diff = 0; //Holds difference in intensity between consecutive pixels
+    prev = -1; //Holds index of last inspected pixel
 
+    //Used for crossroads navigation, holds info on which edges of the track are observed
     leftSeen = false;
     rightSeen = false;
-        
+    
+    //Starting in the middle index, step left, inspecting the the edge of the track
     for(i = 63; i > 0; i--) {
         curr_left = (uint8_t)(cam_data[i] >> 4) & 0xFF;              
         diff = prev - curr_left;
+        //Check incorporates a combination of looking at the difference in intensities
+        //and whether the pixels intensity is less than a threshold, corresponding to the black edge
         if(abs(diff) >= CAM_DIFF && curr_left <= CAM_THRESHOLD && prev != -1) {
-            l = i;
+            l = i; //Record the index where the edge is observed
             leftSeen = true;
             break;
         }
-        prev = curr_left;
+        prev = curr_left; //Update previous value for the loop
     }
     
-        
-    
     prev = -1;
+    //As before, start in the middle but this time step rightwards in the image
     for(i = 64; i < 128; i++) {
         curr_right = (uint8_t)(cam_data[i] >> 4) & 0xFF;
         int diff = prev - curr_right;
@@ -173,26 +178,32 @@
         prev = curr_right;
     }
     
+    //If both edges are not visible, we are likely in a crossroads
     if(!rightSeen && !leftSeen) {
         sendString("lost edges");
-        ALIGN_SERVO;
-        servo_pid.integral = 0;    
+        ALIGN_SERVO; //Straighten wheels so we go straight through the crossroads
+        servo_pid.integral = 0;
     }
     
-    //Calculate how left/right we are   
+    //Calculate how left/right from the centre line we are
     return (64 - ((l+r)/2))/64.f;
 }
 
+//Unused function currently
+//Was used to establish whether we are in a corner, by inspecting a buffer of
+//centre line values
 inline void handleCornering() {
     
+    //Get current value of how left/right of centre line we are on the track
     float lookaheadMeasuredValue = findCentreValue(LOOKAHEAD_CAMERA, farLeft, farRight);
     
     measuredValBuffer[frame_counter % 64] = servo_pid.measured_value;
     
     int count = 0;
     for(i = 0; i < 10; i++) {
+        //Step through the buffer, using modulus operator
         float val = abs(measuredValBuffer[(frame_counter - i) % 64]);
-        if(val > 0.09) {
+        if(val > 0.09) { //If the value exceeds a certain value (obtained experimentally), we are in a corner
             count++;
         }
     } 
@@ -241,6 +252,8 @@
     
 }
 
+//Unused function currently
+//Was used to estimate whether the stop marker was seen
 inline float getLineEntropy() {
     float entropy = 0;
     float last = (int8_t)(CLOSE_CAMERA[0] >> 4) & 0xFF;
@@ -259,7 +272,7 @@
     pid->p_error = pid->pid_error;
     
     if(pid->integral > 1.0f) {
-        pid->integral = 1.0f;   
+        pid->integral = 1.0f;
     }
     if(pid->integral < -1.0f    ) {
         pid->integral = -1.0f;   
@@ -278,7 +291,7 @@
     wR=Get_Speed(Time_R);
     
     // Check if left wheel is slipping/giving an abnormal reading and ignore reading
-    if(wL - prevL < 1.2/0.025) {
+    if(wL - prevL < 1.2/0.025) { //<3 magic numbers: 48....?
         left_motor_pid.measured_value = wL;
     }
     if(wR - prevR < 1.2/0.025) {
@@ -299,7 +312,8 @@
     else //Unhappy PID state
     {        
         //sendString("out = %f p_err = %f", servo_pid.output, servo_pid.p_error);
-        ALIGN_SERVO;
+        //ALIGN_SERVO;
+        //Could cause the car to be travelling along one side of the track rather than in the middle
         if(servo_pid.output >= 1.0f) {
             TFC_SetServo(0, 0.9f);
             servo_pid.output = 1.0f;
@@ -333,57 +347,16 @@
 }
 
 inline void handleStartStop() {
+
+    //Function to detect the NXP cup stop marker
     
-    //v1:
-    //Hacky way to detect the start/stop signal
-    /*if(right - left < 60) {
-        sendString("START STOP!!");
-
-        TFC_SetMotorPWM(0.f,0.f);
-        TFC_HBRIDGE_DISABLE;
-        startstop = 0;
-    }*/
-        
-    //----------------------------START/STOP v2-----------------------------
-    //New method plan:
-    //Start at the centre of the image
-    //Look for 2 transitions (B->W OR W->B) - ignoring whether the centre pixel was black or white
-    //this should efficiently detect whether the marker is visible, and shouldn't give too many false positives
-    //May need to fiddle with the track width initial check, or disbale it entirely.
-    //NB: May want to incorporate this into findCentreValue(), it looks like they will be operating in similar ways on the exact same data...
-    /*
-    uint8_t lastPixel, currentPixel;
-    lastPixel = -1; //or 0?
-    bool startStopLeft = false;
-    bool startStopRight = false
-    //So:
-    //1. Starting at the middle, step left, looking for 2 transitions
-    for(int i = 63; i > 0; i--) {
-        currentPixel = (uint8_t)(CLOSE_CAMERA[i] >> 4) & 0xFF; //Cast to signed or unsigned? Edge detection code has signed, but it puts it in an unsigned variable...
-        if((lastPixel - currentPixel) > 10) { //1st transition
-            for (int j = i; j > 0; j--) { //Keep going until 2nd transition
-                if((lastPixel - currentPixel) > 10) { //2nd transition
-                    //Set flag that 2 transitions on the left side are identified
-                    startStopLeft = true;
-                    //goto finishLeft;
-                }
-            }
-            
-        }
-        startStopLeft = false;
-        lastPixel = currentPixel;
-    }
-    //finishLeft:
-    */
-    
-    //v2.5:--------------
-    
-    int slower = 0;
-    int difference = 0;
+    int slower = 0; //Only send a string every few frames
+    int difference = 0; //Holds the difference between intensities of consecutive pixels
     int lastPixel, currentPixel, transitionsSeen;
     lastPixel = -1;
     transitionsSeen = 0;
-    //Starting  near the left edge, step right, counting transitions. If there are 4, it is the marker (what about 3?)
+    //Starting  near the left edge, step right, counting transitions.
+    //If there are several (exact value varies, best established experimentally), it is the marker
     for(int i = 30; i < 98; i++) {
         currentPixel = (int)(CLOSE_CAMERA[i] >> 4) & 0xFF;
         difference = lastPixel - currentPixel;
@@ -393,9 +366,11 @@
         }
         lastPixel = currentPixel;
     }
+    //Was used to send an indication that the marker was seen, useful for debugging
     //if (slower % 1000 == 0) {
         //sendString("Transitions seen: %d", transitionsSeen);
     //}
+    //slower++;
     if(transitionsSeen >= 5) {
         //Stop the car!
         sendString("Start/stop seen");
@@ -403,8 +378,6 @@
         TFC_HBRIDGE_DISABLE;
         lapTime();
     }
-  //  slower++;
-    
 }