Jorge Montero
RACE CAR
Fork of
TFC-RACING-DEMO
by 
Daniel Hadad
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
| Spices/Spices.cpp | Show annotated file Show diff for this revision Revisions of this file |
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);
}
}