Revision 1:7e3c1d7d50b9, committed 2014-11-17

Comitter:

Jmon14

Date:

Mon Nov 17 20:53:36 2014 +0000

Parent:

0:98e98e01a6ce

Commit message:

Commit message

Changed in this revision

diff -r 98e98e01a6ce -r 7e3c1d7d50b9 Spices/Spices.cpp
--- a/Spices/Spices.cpp	Sun Apr 20 03:33:03 2014 +0000
+++ b/Spices/Spices.cpp	Mon Nov 17 20:53:36 2014 +0000
@@ -8,15 +8,15 @@
 #define NUM_LINE_SCAN 128
 #define MAX_LINE_SCAN NUM_LINE_SCAN-1
 #define MIN_LINE_WIDTH  0
-#define MAX_LINE_WIDTH  15
+#define MAX_LINE_WIDTH  10
 #define FILTER_ENDS 0                              // # of pixels at end of camera data to ignore; set to 0 for now, later make 15
 #define RANGE (NUM_LINE_SCAN - (2 * FILTER_ENDS))  // range of camera pixels to consider
 #define ERR_RATIO 0.85                             // ratio of max possible error to pixels (have to measure!)
 #define DER_RATIO 0.5                              // ratio for der threshold level (was 0.5 initially, may put back)
 
 // steer/servo params
-#define MAX_STEER_LEFT -0.51                       // value determined by demo mode 1 measure (have to be adjusted with every servo horn attach)
-#define MAX_STEER_RIGHT 0.39                       // value determined by demo mode 1 measure
+#define MAX_STEER_LEFT -0.21                       // value determined by demo mode 1 measure (have to be adjusted with every servo horn attach)
+#define MAX_STEER_RIGHT 0.75                       // value determined by demo mode 1 measure
 #define DT 0.02                                    // # MS of time between intervals (doesn't really matter)
 
 // logging parameters
@@ -137,7 +137,10 @@
 
 
 // Steering control variables
-float      CurrentLinePosition;                    // Current position of track line (in pixels -- 0 to 127)
+float      CurrentLinePosition1;                    // Current left position of track line (in pixels -- 0 to 127)
+float      CurrentLinePosition2;                    // Current right position of track line (in pixels -- 0 to 127)
+float      CurrentLinePosition;                      // Current central position of track line (in pixels -- 0 to 127)
+float      TrackWidth = 100;
 float      LastLinePosition;                       // Last position of track line (in pixels -- 0 to 127)
 float      CurrentLinePosError = 0;                // Current line position error (used for derivative calc)
 float      AbsError;
@@ -223,7 +226,7 @@
   // Every 4s (or Terminal Output is off, i.e. race mode!)
   //    AND line scan image ready (or fake mode where image is always ready)
   //   (ticker updates every 2ms) (Line scan image ready very 20mS?)
-  if((TFC_Ticker[0]>2000 || (terminalOutput != 3)) && (TFC_LineScanImageReady>0 || debugFakeMode))
+  if((TFC_Ticker[0]>5000 || (terminalOutput != 3)) && (TFC_LineScanImageReady>0 || debugFakeMode))
    {
 
      // stuff that needs to be reset with each image frame
@@ -303,12 +306,9 @@
      
      // ** Track Status available at this point **  
       
-     
-     // test out accelerometer
-    // accelTest();
-      
      // Update things based on latest track status
      // e.g. change steering setting, stop car, ...
+     
      ActOnTrackStatus();
      
      if (terminalOutput == 2) {
@@ -316,8 +316,7 @@
        TERMINAL_PRINTF("TIME:TO AFTER ActOnTrackStatus:%d:uSec\r\n", after_time - before_time);
        before_time = timer.read_us();
      }      
-
-
+     
      //give LED feedback as to track status
      feedbackLights();
      
@@ -751,69 +750,128 @@
       else
         TERMINAL_PRINTF(", ");
     }
-
+    
+    TERMINAL_PRINTF("Left Line Found:");
+    TERMINAL_PRINTF("%3.1f", CurrentLinePosition1);
+    TERMINAL_PRINTF("\r\n");
+    TERMINAL_PRINTF("Right Line Found:");
+    TERMINAL_PRINTF("%3.1f", CurrentLinePosition2);
+    TERMINAL_PRINTF("\r\n");
+    TERMINAL_PRINTF("Track Width:");
+    TERMINAL_PRINTF("%3.1f", TrackWidth);
+    TERMINAL_PRINTF("\r\n");    
 }
 
 void reviewEdges()
 {
   LastTrackStatus = CurrentTrackStatus;
   
-  if ((numPosEdges == 1) && (numNegEdges == 1))  // only one negative and positive edge found (LINE)
+  if ((numPosEdges == 1) && (numNegEdges == 1))  // only one negative and positive edge found
   {
-    if (((PosEdges[0] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[0] - NegEdges[0]) <= MAX_LINE_WIDTH)) // has proper expected width
+    if (((PosEdges[0] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[0] - NegEdges[0]) <= MAX_LINE_WIDTH)) // has proper expected width (1 line found)
     {
-       CurrentTrackStatus = LineFound;                                   // report line found!
-       UnknownCount = 0;                                          // reset unknown status count
-       LastLinePosition = CurrentLinePosition;
-       CurrentLinePosition = (PosEdges[0]+NegEdges[0]) / 2;       // update line position
+       if((LastLinePosError > 0))
+        {
+            CurrentTrackStatus = LineFound;                                   // report line found!
+            UnknownCount = 0;                                          // reset unknown status count
+            LastLinePosition = CurrentLinePosition;
+            CurrentLinePosition1 = (PosEdges[0]+NegEdges[0]) / 2;       // update line position
+            CurrentLinePosition2 = CurrentLinePosition1 + TrackWidth; // mayor a 128
+            CurrentLinePosition = (CurrentLinePosition1 + CurrentLinePosition2)/2;
+        }
+        
+        else
+        {
+            CurrentTrackStatus = LineFound;                                   // report line found!
+            UnknownCount = 0;                                          // reset unknown status count
+            LastLinePosition = CurrentLinePosition;
+            CurrentLinePosition2 = (PosEdges[0]+NegEdges[0]) / 2;       // update line position
+            CurrentLinePosition1 = CurrentLinePosition2 - TrackWidth; // menor a 128
+            CurrentLinePosition = (CurrentLinePosition1 + CurrentLinePosition2)/2;
+        }
     }
-  } else if ((numPosEdges == 1) && (numNegEdges == 0))  {     // 1 pos edge found (POSSIBLE LINE)
-    if ((PosEdges[0] <= MAX_LINE_WIDTH) && (LastLinePosError < 0))       // pos edge is within line width of edge of camera (LEFT)
+  } 
+  
+  else if ((numPosEdges == 2) && (numNegEdges == 1))  // 2 posEdges 1 negEdges (posible left line)
+  {     
+    if ((PosEdges[0] <= MAX_LINE_WIDTH) && ((PosEdges[1] - NegEdges[0]) <= MAX_LINE_WIDTH) )       // Maybe left line and right lane
     {
-       CurrentTrackStatus = LineFound;                                   // report line found!
-       UnknownCount = 0;                                                 // reset unknown status count
-       LastLinePosition = CurrentLinePosition;
-       CurrentLinePosition = PosEdges[0] - ( MAX_LINE_WIDTH / 2);        // update line position
-     //  TERMINAL_PRINTF("*** SINGLE POSEDGE LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition);
+       if(((NegEdges[0]-PosEdges[0]) > (TrackWidth - 15)) && ((NegEdges[0]-PosEdges[0]) < (TrackWidth + 15))) // 2 lines
+       {
+            CurrentTrackStatus = LineFound;                                   // report line found!
+            UnknownCount = 0;                                                 // reset unknown status count
+            LastLinePosition = CurrentLinePosition;
+            CurrentLinePosition1 = PosEdges[0] - ( MAX_LINE_WIDTH / 2);        // update line position
+            CurrentLinePosition2 = (PosEdges[1] + NegEdges[0])/2;
+            TrackWidth = CurrentLinePosition2 - CurrentLinePosition1;
+            CurrentLinePosition = (CurrentLinePosition1 + CurrentLinePosition2)/2;
+        }
     }
-  } else if ((numNegEdges == 1) && (numPosEdges == 0))  {     // 1 neg edge found (POSSIBLE LINE)
-    if ((NegEdges[0] >= (MAX_LINE_SCAN - MAX_LINE_WIDTH)) && (LastLinePosError > 0))    // neg edge is within line width of edge of camera (RIGHT)
+  } 
+  
+  else if ((numNegEdges == 2) && (numPosEdges == 1))  // 2 neg edge 1 posEdge found (POSSIBLE LINE right)
+  {     
+    if ( (NegEdges[1] >= (NUM_LINE_SCAN - MAX_LINE_WIDTH) ) && ((PosEdges[0] - NegEdges[0]) <= MAX_LINE_WIDTH))    // neg edge is within line width of edge of camera (RIGHT)
     {
-       CurrentTrackStatus = LineFound;                                   // report line found!
-       UnknownCount = 0;                                                 // reset unknown status count
-       LastLinePosition = CurrentLinePosition;
-       CurrentLinePosition = NegEdges[0] + ( MAX_LINE_WIDTH / 2);        // update line position
-    //   TERMINAL_PRINTF("*** SINGLE NEGEDGE LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition);
-    } 
-  } else if ((numPosEdges == 2) && (numNegEdges == 2))  {     // 2 negative and 2 positive edges found (STARTING/FINISH GATE)
+       if(((NegEdges[1]-PosEdges[0]) > (TrackWidth - 15)) && ((NegEdges[1]-PosEdges[0]) < (TrackWidth + 15))) // 2 lines
+       {    
+            CurrentTrackStatus = LineFound;                                   // report line found!
+            UnknownCount = 0;                                                 // reset unknown status count
+            LastLinePosition = CurrentLinePosition;
+            CurrentLinePosition1 = (PosEdges[0] + NegEdges[0])/2;        // update line position
+            CurrentLinePosition2 = (NegEdges[1] + MAX_LINE_WIDTH/2);
+            TrackWidth = CurrentLinePosition2 - CurrentLinePosition1;
+            CurrentLinePosition = (CurrentLinePosition1 + CurrentLinePosition2)/2;            
+        } 
+    }
+  } 
+  
+  else if ((numPosEdges == 2) && (numNegEdges == 2))  // 2 negative and 2 positive edges found (STARTING/FINISH GATE)
+  {     
   
-    if ((((NegEdges[0] - PosEdges[0]) >= MIN_LINE_WIDTH) && ((NegEdges[0] - PosEdges[0]) <= MAX_LINE_WIDTH)) &&    // white left 'line'
-        (((NegEdges[1] - PosEdges[1]) >= MIN_LINE_WIDTH) && ((NegEdges[1] - PosEdges[1]) <= MAX_LINE_WIDTH)) &&    // white right 'line'
-        (((PosEdges[1] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[1] - NegEdges[0]) <= MAX_LINE_WIDTH))       // actual track line
-        )
+    if ( ( ( (PosEdges[0] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[0] - NegEdges[0]) <= MAX_LINE_WIDTH)) &&    // white left 'line'
+        (((PosEdges[1] - NegEdges[1]) >= MIN_LINE_WIDTH) && ((PosEdges[1] - NegEdges[1]) <= MAX_LINE_WIDTH)) &&    // white right 'line'
+        ( ( (NegEdges[1] - PosEdges[0]) >= (TrackWidth - 15) ) && ((NegEdges[1] - PosEdges[0]) <= (TrackWidth + 15) ) ) )      // actual track line
+    {       
+    
+        CurrentTrackStatus = LineFound;
+        UnknownCount = 0;                                            // reset unknown status count
+        CurrentLinePosition1 = (PosEdges[0] + NegEdges[0])/2;        // update line position
+        CurrentLinePosition2 = (PosEdges[1] + NegEdges[1])/2;
+        TrackWidth = CurrentLinePosition2 - CurrentLinePosition1;
+        CurrentLinePosition = (CurrentLinePosition1 + CurrentLinePosition2)/2;      
+    }
            
-       
-    if (startRaceTicker > STARTGATEDELAY) {                      // only start counting for starting gate until after delay
+  } 
+  
+  else if ((numPosEdges > 2) && (numNegEdges > 2)) // more than 1 negative edge and positive edge found (but not 2 for both) (STARTING / FINISH GATE)
+  {   
+   
+   // remove edges that aren't close to center TBD DDHH  
+    
+    if (startRaceTicker > STARTGATEDELAY) // only start counting for starting gate until after delay
+    {                      
       StartGateFoundCount++;
     }
-    
-    CurrentTrackStatus = StartGateFound;
-    UnknownCount = 0;                                            // reset unknown status count
-           
-  } else if ((numPosEdges > 1) && (numNegEdges > 1)) {   // more than 1 negative edge and positive edge found (but not 2 for both) (STARTING / FINISH GATE)
-  
-   // remove edges that aren't close to center TBD DDHH
    
-      if (terminalOutput == 3) {
+      if (terminalOutput == 3) 
+      {
          TERMINAL_PRINTF("***************************************** \r\n");
          TERMINAL_PRINTF("********** NOT SURE FOUND ********** \r\n");
          TERMINAL_PRINTF("***************************************** \r\n");
-       } 
-    CurrentTrackStatus = Unknown; 
+      } 
+       
+    CurrentTrackStatus = StartGateFound; 
+  
+  } 
   
-  } else {  // no track or starting gate found
+  else // no track or starting gate found
+  
+  {  
   
-    if (terminalOutput == 3) {
+    if (terminalOutput == 3) 
+    
+    {
       TERMINAL_PRINTF("***************************************** \r\n");
       TERMINAL_PRINTF("*** !!!!!!!!!! LINE NOT FOUND !!!!!!! *** \r\n", CurrentLinePosition);
       TERMINAL_PRINTF("***************************************** \r\n");
@@ -822,10 +880,6 @@
     CurrentTrackStatus = Unknown;
     UnknownCount++;
   }
-  
-  
-
-
 }
 
 void ActOnTrackStatus()
@@ -864,9 +918,6 @@
     }
   
   }
-  
-
-
 }
 
 void SteeringControl()
@@ -911,6 +962,7 @@
       KP = TUNE_KP;
       KI = TUNE_KI;
       KD = TUNE_KD;
+      break;
     case 3:
       if (AbsError < ABS_ERROR_THRESH) {
         KP = 0.003;  // when relatively straight, keep KP gain low
@@ -919,6 +971,7 @@
       }
       KI = 0;
       KD = 0;
+      break;
       
     default:
       break;
@@ -979,7 +1032,7 @@
 
     // Finally add offset to steering to account for non-centered servo mounting
     // CurrentSteerSetting = Pout + Iout + Dout + ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 );
-    CurrentSteerSetting = Pout + ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 );
+    CurrentSteerSetting = ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 ) - Pout ;
     
     // store for next cycle deriv calculation
     LastLinePosError = CurrentLinePosError;
@@ -1151,7 +1204,7 @@
   if (terminalOutput == 3) {
     TERMINAL_PRINTF("Abs Error: %4.2f\r\n", AbsError);
     TERMINAL_PRINTF("Error Limit: %4.2f\r\n", ErrLimit);
-    TERMINAL_PRINTF("MAX SPEED = %5.2f\n", MaxSpeed);
+    TERMINAL_PRINTF("MAX SPEED = %5.2f\r\n", MaxSpeed);
     TERMINAL_PRINTF("Current Left Drive Setting: %5.2f\r\n", CurrentLeftDriveSetting);
     TERMINAL_PRINTF("Current Right Drive Setting: %5.2f\r\n", CurrentRightDriveSetting);
   }
@@ -1238,7 +1291,8 @@
   if (terminalOutput == 3) {
     TERMINAL_PRINTF("Max Light Intensity: %4d\r\n", MaxLightIntensity);
     TERMINAL_PRINTF("Min Light Intensity: %4d\r\n", MinLightIntensity);
-    TERMINAL_PRINTF("Deriv Threshold: %9.3f\r\n", DerivThreshold);
+    TERMINAL_PRINTF("Deriv Threshold: %9.3f\r\n", NegDerivThreshold);
+    TERMINAL_PRINTF("Deriv Threshold: %9.3f\r\n", PosDerivThreshold);
   }
 
 }