Keith Cyr
TFC-Mentoring-Matters-Abstraction
RaceCar/TheDetails.cpp
- Committer:
- mrkeithcyr
- Date:
- 2014-07-16
- Revision:
- 0:0699eb71778e
File content as of revision 0:0699eb71778e:
#include "TFC.h"
#include "mbed.h"
/****** INCLUDES ******/
#include "mbed.h"
#include "common.h"
#include "TFC.h"
/**********************/
/****** CAMERA PARAMETERS ******/
#define NUM_LINE_SCAN 128
#define MAX_LINE_SCAN NUM_LINE_SCAN-1
#define MIN_LINE_WIDTH 0
#define MAX_LINE_WIDTH 15
// number of pixels at end of camera data to ignore
// set to 0 for now, later make 15
#define FILTER_ENDS 15
// range of camera pixels to consider
#define RANGE (NUM_LINE_SCAN - (2 * FILTER_ENDS))
// ratio of max possible error to pixels (have to measure!)
#define ERR_RATIO 0.85
// ratio for der threshold level (was 0.5 initially, may put back)
#define DER_RATIO 0.5
/*******************************/
/****** STEERING AND SERVO PARAMETERS ******/
// value determined by garage mode 2 measure
// must be adjusted for every servo horn attached
// must be re-adjusted (or at least checked) if
#define MAX_STEER_LEFT -0.38
// value determined by garage mode 2 measure
#define MAX_STEER_RIGHT 0.49
// # MS of time between intervals (doesn't really matter)
#define DT 0.02
/*******************************************/
/****** LOGGING PARAMETERS ******/
// number of frames to log (when logging is active) ~14 sec worth!
#define NUM_LOG_FRAMES 700
/********************************/
/****** FOR DEBUG TUNING ******/
#define TUNE_KP 0.008
#define TUNE_KI 0
#define TUNE_KD 0
// percent minimum power
// estimating for a 2-ft turn => 24" / (24" + 6" car) = 4/5
// speed of inner wheel is 20% lower worst case
#define MIN_POWER 60
// do not change
#define SPEED_ADJUST 4
// number of pixels line position offset before changing KP value
#define ABS_ERROR_THRESH 10
// which control method to use
#define CONTROL_METHOD 1
/******************************/
/****** DRIVE/MOTOR PARAMETERS ******/
// maximum speed cap for slow speed setting
#define LIGHT_SPEED 0.5
// maximum speed cap for faster speed setting
#define LUDICROUS_SPEED 0.7
// percent max power (for speed adjustments)
#define MAX_POWER 100
/************************************/
/****** ALGORITHM PARAMETERS ******/
// max value to allow for unknown track conditions before killing engine
#define UNKNOWN_COUNT_MAX 50
// max value to allow for finding starting gate before killing engine
#define STARTGATEFOUNDMAX 0
// delay before searching for starting gate to kill engine
#define STARTGATEDELAY 50
/**********************************/
/****** IMAGE PROCESSING VARIABLES ******/
// snapshot of camera data for this 'frame'
uint16_t GrabLineScanImage0[NUM_LINE_SCAN];
// derivative of line scan data
float DerivLineScanImage0[NUM_LINE_SCAN];
// array-- set of where in line scan data negative edges found
float NegEdges[NUM_LINE_SCAN];
// array-- set of where in line scan data positive edges found
float PosEdges[NUM_LINE_SCAN];
// max value of valid neg and positive indices
// also serves as a count of # edges found
uint16_t numNegEdges = 0, numPosEdges = 0;
// maximum measured light intensity -- to account for lighting differences
uint16_t MaxLightIntensity = 0;
// minimum measured light intensity -- to account for lighting differences
uint16_t MinLightIntensity = (1 << 12);
// maximum derivative value
float maxDerVal = 0;
// minimum derivative value
float minDerVal = (float) (1 << 12);
// average derivative value
float aveDerVal = 0;
// default derivative threshold
float DerivThreshold = (1 << 9);
// default positive edge derivative threshold
float PosDerivThreshold = (1 << 9);
// default negative edge derivative threshold
float NegDerivThreshold = (1 << 9);
/****************************************/
/****** STEERING CONTROL VARIABLES ******/
// current position of track line (in pixels -- 0 to 127)
float CurrentLinePosition;
// last position of track line (in pixels -- 0 to 127)
float LastLinePosition;
// current line position error (used for derivative calculation)
float CurrentLinePosError = 0;
float AbsError;
// last line position error (used for derivative calculation)
float LastLinePosError = 0;
// sum of line position error (used for integral calculation)
float SumLinePosError = 0;
// derivative of the error
float DerivError = 0;
// drive straight at first
float CurrentSteerSetting = (MAX_STEER_RIGHT + MAX_STEER_LEFT) / 2;
// left wheel drive setting
float CurrentLeftDriveSetting = 0;
// right wheel drive setting
float CurrentRightDriveSetting = 0;
/****************************************/
/****** SPEED CONTROL VARIABLES ******/
// maximum speed allowed
float MaxSpeed;
// ticker at start of race
uint16_t startRaceTicker;
/*************************************/
/****** CUSTOM DATA TYPES ******/
typedef enum TrackStatusType {Unknown,
LineFound,
StartGateFound,
LineJustLeft} TrackStatusType;
TrackStatusType CurrentTrackStatus; // current track status
TrackStatusType LastTrackStatus; // last track status
/* typedef enum TrackType {NotSure,
Straight,
Curve,
Wiggle,
Bumps,
StartGate,
UpHill,
DownHill} TrackType;
TrackType CurrentTrack; */
struct LogData {
float linepos;
float steersetting;
float leftdrivesetting;
float rightdrivesetting;
};
// array of log data to store
LogData frameLogs[NUM_LOG_FRAMES];
// index for log data
int logDataIndex;
// how many times start gate has been found
int StartGateFoundCount = 0;
// how many times nothing has been found
// help5 with kill switch implementation
int UnknownCount = 0;
// Car can go! Should be set to false to start.
bool go = false;
/*******************************/
/****** EXTRA CONTROL PARAMETERS ******/
// if true, ignores real camera and uses fake camera input instead
// used for data processing debug
extern bool debugFakeMode = false;
// whether output terminal data
bool terminalOutput = false;
// whether to capture log data to output later on
bool doLogData = false;
// whether to enable Kill Switch
// allows engine to stop after not finding track if true
bool killSwitch = false;
// whether to stop or not depending on starting gate reading
bool startGateStop = false;
// race style -- whether conservative or risky
bool doRisky = false;
/**************************************/
/****** terminalMode() *********************************************************
Purpose: Checks the current setting of dip switch 4.
Parameters: None
Returns: True if dip switch 4 is set.
*******************************************************************************/
bool terminalMode()
{
return ((TFC_GetDIP_Switch()>>3)&0x01 == 1);
}
/****** GiveDriveSettingsToMotors() ********************************************
Purpose: Simply passes parameters to TFC_SetMotorPWM().
Parameters: The desired settings for each of the two motors
Returns: None
*******************************************************************************/
void GiveDriveSettingsToMotors(float right_drive_setting,
float left_drive_setting)
{
TFC_SetMotorPWM(right_drive_setting, left_drive_setting);
}
/****** GiveSteeringSettingTo() ************************************************
Purpose: Simply passes parameters to TFC_SetServo().
Parameters: The servo number and the steering setting.
Returns: None
*******************************************************************************/
void GiveSteeringSettingTo(uint8_t ServoNumber, float Position)
{
TFC_SetServo(ServoNumber, Position);
}
/****** ResetCamera() **********************************************************
Purpose: This function resets the camera after an image has been used.
Parameters: None
Returns: None
*******************************************************************************/
void ResetCamera()
{
TFC_LineScanImageReady = false;
}
/****** ResetMaximumLightReading() *********************************************
Purpose: Resets the maximum light intensity variable
Parameters: None
Returns: None
*******************************************************************************/
void ResetMaximumLightReading()
{
MaxLightIntensity = 0;
}
/****** ResetMinimumLightReading() *********************************************
Purpose: Resets the minimum light intensity variable
Parameters: None
Returns: None
*******************************************************************************/
void ResetMinimumLightReading()
{
MinLightIntensity = (1 << 12);
}
/****** grabCameraFrame() ******************************************************
Purpose: This function gets a scan from the camera or fakes a scan from the
camera. There are different types of fake scans that can be used to test
other parts of the car to see how the car reacts to certain types of line
conditions.
Parameters: None
Returns: None
*******************************************************************************/
void grabCameraFrame()
{
uint32_t i = 0;
uint8_t fake_type = 4; // type of fake data if used
for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data
// (128 characters) from Camera 0
{
if (debugFakeMode)
{ // use fake camera data
switch (fake_type)
{
// ideal track -- line in center
case 0:
if (i<57 || i > 70)
GrabLineScanImage0[i] = 0xFFF; // no line
else
GrabLineScanImage0[i] = 0x4B0; // line
break;
// ideal track -- line to the left
case 1:
if (i<27 || i > 40)
GrabLineScanImage0[i] = 0xFFF; // no line
else
GrabLineScanImage0[i] = 0x4B0; // line
break;
// ideal track -- line to the right
case 2:
if (i<87 || i > 100)
GrabLineScanImage0[i] = 0xFFF; // no line
else
GrabLineScanImage0[i] = 0x4B0; // line
break;
// ideal track -- starting gate!
case 3:
// TBD
break;
// less than ideal track -- debug multi-edge issue!
case 4:
if (i<54)
GrabLineScanImage0[i] = 4000; // no line
if (i == 54)
GrabLineScanImage0[i] = 3370; // neg edge
if (i == 55)
GrabLineScanImage0[i] = 3309; // neg edge
if (i == 56)
GrabLineScanImage0[i] = 2016; // neg edge
if (i == 57)
GrabLineScanImage0[i] = 711; // neg edge
if (i == 58)
GrabLineScanImage0[i] = 696; // neg edge
if ((i>58) && (i<69))
GrabLineScanImage0[i] = 500; // line
if (i == 69)
GrabLineScanImage0[i] = 1800; // pos edge
if (i > 69)
GrabLineScanImage0[i] = 4000; // no line
default:
break;
}
} else // use real camera data
{
GrabLineScanImage0[i] = TFC_LineScanImage0[i];
}
}
}
/****** GetTheImageToUse() *****************************************************
Purpose: Retrieves whichever image we are supposed to use.
Parameters: None
Returns: None
*******************************************************************************/
void GetTheImageToUse()
{
grabCameraFrame();
}
/****** derivativeLineScan() ***************************************************
Purpose: Calculates derivative of line scan data.
Parameters: Pointer to current image scan data, and a pointer to where you want
the derivative to be output.
Returns: None
*******************************************************************************/
void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut)
{
uint32_t i;
uint32_t minCnt = 0;
uint32_t maxCnt = 0;
float DerVal;
float upperDerVal;
float lowerDerVal = 0;
maxDerVal = 0;
minDerVal = (float) (1 << 12);
aveDerVal = 0;
minCnt = FILTER_ENDS;
maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
// TERMINAL_PRINTF("i, upperDerVal, lowerDerVal, DerVal\r\n");
// print one line worth of data from Camera 0
for(i=minCnt;i<maxCnt;i++)
{
// store max light intensity value
if (LineScanDataIn[i] > MaxLightIntensity)
MaxLightIntensity = LineScanDataIn[i];
// store min light intensity value
if (LineScanDataIn[i] < MinLightIntensity)
MinLightIntensity = LineScanDataIn[i];
// Central Derivative
// start point
if (i==minCnt)
{
upperDerVal = (float)(LineScanDataIn[i+1]);
// make same as start point
lowerDerVal = (float)(LineScanDataIn[i]);
}
// end point
else if (i==maxCnt - 1)
{
// make same as end point
upperDerVal = (float)(LineScanDataIn[i]);
lowerDerVal = (float)(LineScanDataIn[i-1]);
}
// any other point
else
{
upperDerVal = (float)(LineScanDataIn[i+1]);
lowerDerVal = (float)(LineScanDataIn[i-1]);
}
DerVal = (upperDerVal - lowerDerVal) / 2;
// TERMINAL_PRINTF("%d,%9.3f,%9.3f,%9.3f\r\n",
// i, upperDerVal, lowerDerVal, DerVal);
if (DerVal > maxDerVal)
{
maxDerVal = DerVal;
}
if (DerVal < minDerVal)
{
minDerVal = DerVal;
}
//get sum
aveDerVal = aveDerVal + DerVal;
DerivLineScanDataOut[i] = DerVal;
}
aveDerVal = (float) aveDerVal / (maxCnt - minCnt);
}
/****** GetCameraData() *****************************************************
Purpose: Processes an image to get the camera's raw data.
Parameters: None
Returns: None
*******************************************************************************/
void GetCameraData()
{
derivativeLineScan(&GrabLineScanImage0[0], &DerivLineScanImage0[0]);
}
/****** printAdjustLightsData() ************************************************
Purpose: Prints out light intensity data to the terminal.
Parameters: None
Returns: None
*******************************************************************************/
void printAdjustLightsData()
{
if (terminalOutput)
{
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);
}
}
/****** adjustLights() *********************************************************
Purpose: Adjusts the line found threshold for different lighting conditions.
Parameters: None
Returns: None
*******************************************************************************/
void adjustLights()
{
// LIGHT ADJUST METHOD 1
// threshold is 1/5 of light intensity 'range'
if (1 == 0) {
DerivThreshold = (float) (MaxLightIntensity - MinLightIntensity) / 5;
NegDerivThreshold = (float) -1 * (DerivThreshold);
PosDerivThreshold = (float) (DerivThreshold);
}
// LIGHT ADJUST METHOD 2 -- SEEMS TO WORK MUCH BETTER
// pos edge threshold is half range of max derivative above ave derivative
// neg edge threshold is half range of min derivative above ave derivative
else
{
NegDerivThreshold = (float) (minDerVal - aveDerVal) * DER_RATIO;
PosDerivThreshold = (float) (maxDerVal - aveDerVal) * DER_RATIO;
}
printAdjustLightsData();
}
/****** findEdges_v2() *********************************************************
Purpose: This function is used to determine where the edges of the line are
found in the camera image. The location of the line edges is printed out to
the terminal if terminal output has been enabled.
Parameters: Pointer to the array of line scan data derivatives
Returns: None
*******************************************************************************/
void findEdges_v2(float* derivLineScanData)
{
// search for edges in deriviative data using a threshold
// need to store in a hash if that's possible...
// combine edges that are a pixel apart
int i;
// serves as buffer to store neg edges found next to each other
int NegEdgeBufCnt = 0, NegEdgeBufSum = 0;
// serves as buffer to store pos edges found next to each other
int PosEdgeBufCnt = 0, PosEdgeBufSum = 0;
int minCnt = FILTER_ENDS;
int maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
// count of neg edges found thus far
numNegEdges = 0;
// count of pos edges found thus far
numPosEdges = 0;
// print one line worth of data from Camera 0
for(i=minCnt;i<maxCnt;i++)
{
// NEGATIVE EDGE FOUND!
if (derivLineScanData[i] <= NegDerivThreshold)
{
if (terminalOutput)
{
TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX "
"%d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
}
// add value to neg edge buffer
NegEdgeBufCnt++;
NegEdgeBufSum = NegEdgeBufSum + i;
}
// POSITIVE EDGE FOUND!
else if(derivLineScanData[i] > PosDerivThreshold)
{
if (terminalOutput)
{
TERMINAL_PRINTF("POS EDGE FOUND AT INDEX "
"%d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
}
// add value to pos edge buffer
PosEdgeBufCnt++;
PosEdgeBufSum = PosEdgeBufSum + i;
}
// NO EDGE FOUND
else
{
// POP EDGE BUFFERS IF NON-EMPTY AND STORE TO EDGE "STACK"
// (i.e. edges found)
if (NegEdgeBufCnt > 0)
{
// store edge value
numNegEdges++;
NegEdges[numNegEdges - 1] =
(float) NegEdgeBufSum / NegEdgeBufCnt;
// clear edge buffer
NegEdgeBufSum = 0; NegEdgeBufCnt = 0;
}
if (PosEdgeBufCnt > 0)
{
// store edge value
numPosEdges++;
PosEdges[numPosEdges - 1] =
(float) PosEdgeBufSum / PosEdgeBufCnt;
// clear edge buffer
PosEdgeBufSum = 0; PosEdgeBufCnt = 0;
}
}
}
}
/****** FindTheLine() *********************************************************
Purpose: Simply a link to whichever version of the edge-finding functions you
choose.
Parameters: None
Returns: None
*******************************************************************************/
void FindTheLine()
{
findEdges_v2(&DerivLineScanImage0[0]);
}
/****** reviewEdges() **********************************************************
Purpose: This function check which edges were found and decides whether or not
the line has been found. Its results are printed out to the terminal if
terminal output has been enabled.
Parameters: None
Returns: None
*******************************************************************************/
void reviewEdges()
{
LastTrackStatus = CurrentTrackStatus;
// only one negative and positive edge found (LINE)
if ((numPosEdges == 1) && (numNegEdges == 1))
{
// has proper expected width
if (((PosEdges[0] - NegEdges[0]) >= MIN_LINE_WIDTH)
&& ((PosEdges[0] - NegEdges[0]) <= MAX_LINE_WIDTH))
{
// report line found!
CurrentTrackStatus = LineFound;
// reset unknown status count
UnknownCount = 0;
LastLinePosition = CurrentLinePosition;
// update line position
CurrentLinePosition = (PosEdges[0]+NegEdges[0]) / 2;
}
}
// 1 pos edge found (POSSIBLE LINE)
else if ((numPosEdges == 1) && (numNegEdges == 0))
{
// pos edge is within line width of edge of camera (LEFT)
if ((PosEdges[0] <= MAX_LINE_WIDTH) && (LastLinePosError < 0))
{
// report line found!
CurrentTrackStatus = LineFound;
// reset unknown status count
UnknownCount = 0;
LastLinePosition = CurrentLinePosition;
// update line position
CurrentLinePosition = PosEdges[0] - ( MAX_LINE_WIDTH / 2);
// TERMINAL_PRINTF("*** SINGLE POSEDGE LINE FOUND AT POSITION "
// "%9.3f *** \r\n", CurrentLinePosition);
}
}
// 1 neg edge found (POSSIBLE LINE)
else if ((numNegEdges == 1) && (numPosEdges == 0))
{
// neg edge is within line width of edge of camera (RIGHT)
if ((NegEdges[0] >= (MAX_LINE_SCAN - MAX_LINE_WIDTH))
&& (LastLinePosError > 0))
{
// report line found!
CurrentTrackStatus = LineFound;
// reset unknown status count
UnknownCount = 0;
LastLinePosition = CurrentLinePosition;
// update line position
CurrentLinePosition = NegEdges[0] + ( MAX_LINE_WIDTH / 2);
// TERMINAL_PRINTF("*** SINGLE NEGEDGE LINE FOUND AT POSITION "
// "%9.3f *** \r\n", CurrentLinePosition);
}
}
// 2 negative and 2 positive edges found (STARTING/FINISH GATE)
else if ((numPosEdges == 2) && (numNegEdges == 2))
{
if ( // white left 'line'
(((NegEdges[0] - PosEdges[0]) >= MIN_LINE_WIDTH)
&& ((NegEdges[0] - PosEdges[0]) <= MAX_LINE_WIDTH))
// white right 'line'
&& (((NegEdges[1] - PosEdges[1]) >= MIN_LINE_WIDTH)
&& ((NegEdges[1] - PosEdges[1]) <= MAX_LINE_WIDTH))
// actual track line
&& (((PosEdges[1] - NegEdges[0]) >= MIN_LINE_WIDTH)
&& ((PosEdges[1] - NegEdges[0]) <= MAX_LINE_WIDTH)))
{
// only start counting for starting gate until after delay
if (startRaceTicker > STARTGATEDELAY)
{
StartGateFoundCount++;
}
}
CurrentTrackStatus = StartGateFound;
// reset unknown status count
UnknownCount = 0;
}
// more than 1 negative edge and positive edge found
// (but not 2 for both) (STARTING / FINISH GATE)
else if ((numPosEdges > 1) && (numNegEdges > 1))
{
// remove edges that aren't close to center TBD DDHH
if (terminalOutput)
{
TERMINAL_PRINTF("***************************************** \r\n");
TERMINAL_PRINTF("********** NOT SURE FOUND ********** \r\n");
TERMINAL_PRINTF("***************************************** \r\n");
}
CurrentTrackStatus = Unknown;
}
// no track or starting gate found
else
{
if (terminalOutput) {
TERMINAL_PRINTF("***************************************** \r\n");
TERMINAL_PRINTF("*** !!!!!!!!!! LINE NOT FOUND !!!!!!! *** \r\n",
CurrentLinePosition);
TERMINAL_PRINTF("***************************************** \r\n");
}
CurrentTrackStatus = Unknown;
UnknownCount++;
}
}
/****** printLineScanData() ****************************************************
Purpose: This function prints out the camera data to the terminal for use in
testing and debugging.
Parameters: Takes the most recent scan returned by the camera.
Returns: None
*******************************************************************************/
void printLineScanData(uint16_t* LineScanData)
{
uint32_t i = 0;
float Val;
TERMINAL_PRINTF("LINE SCAN DATA:,");
// print one line worth of data (128) from Camera 0
for(i=0;i<NUM_LINE_SCAN;i++)
{
if (1 == 1) { // use float to print
Val = (float) LineScanData[i];
TERMINAL_PRINTF("%9.3f",Val);
if(i==MAX_LINE_SCAN) // when last data reached put in line return
TERMINAL_PRINTF("\r\n");
else
TERMINAL_PRINTF(",");
} else
{
TERMINAL_PRINTF("0x%X",LineScanData[i]);
if(i==MAX_LINE_SCAN) // when last data reached put in line return
TERMINAL_PRINTF("\r\n",LineScanData[i]);
else
TERMINAL_PRINTF(",",LineScanData[i]);
}
}
}
/****** SteeringControl() ******************************************************
Purpose: This function decides how much to turn the front wheels to keep the car
on course.
The CurrentSteerSetting variable is set here.
Its results are printed out to the terminal if terminal output has been
enabled.
Parameters: None
Returns: None
*******************************************************************************/
void SteeringControl()
{
float ReadPot1 = 1;
// target to achieve for line position
float targetPosition = (float)( (NUM_LINE_SCAN / 2) - 0.5);
float KP; // proportional control factor
float KI; // integral control factor
float KD; // derivative control factor
// PID terms
float Pout;
float Iout;
float Dout;
// Calculate error
// make error to the right positive
// i.e. if LINE to the right-- then CurrentLinePosError > 0
// if LINE to the left -- then CurrentLinePosError < 0
CurrentLinePosError = CurrentLinePosition - targetPosition;
// Get absolute error
if (CurrentLinePosError >= 0)
AbsError = CurrentLinePosError;
else
AbsError = -1 * CurrentLinePosError;
// CHOOSE SET OF PID CONTROL PARAMETERS
switch (CONTROL_METHOD)
{
// Pure proportional control based on
// range of steering values vs. range of error values
case 0:
KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT )
/ ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH );
KD = 0;
KI = 0;
break;
// Proportional control but 50% more aggressive around the bends
case 1:
ReadPot1 = TFC_ReadPot(1)+1; // pot range 0-2
KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT )
/ ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH );
KP = KP * ReadPot1;
KD = 0;
KI = 0;
break;
// MANUAL TUNING CASE 1 (use pot to help determine tuning parameters)
case 2:
KP = TUNE_KP;
KI = TUNE_KI;
KD = TUNE_KD;
case 3:
if (AbsError < ABS_ERROR_THRESH)
{
KP = 0.003; // when relatively straight, keep KP gain low
}
else
{
KP = 0.010; // when curve begins or off track, increase KP gain
}
KI = 0;
KD = 0;
default:
break;
}
/* Pseudocode
previous_error = 0
integral = 0
start:
error = setpoint - measured_value
integral = integral + error*dt
derivative = (error - previous_error)/dt
output = Kp*error + Ki*integral + Kd*derivative
previous_error = error
wait(dt)
goto start
*/
if (terminalOutput)
{
TERMINAL_PRINTF("KP = %6.4f\r\n", KP);
TERMINAL_PRINTF("TARGET %6.3f\r\n", targetPosition);
}
// Update integral of error
// i.e. if LINE stays to the right, then SumLinePosError increases
// i.e. if LINE stays to the left, then SumLinePosError decreases
SumLinePosError = SumLinePosError + ( CurrentLinePosError * DT );
DerivError = (CurrentLinePosError - LastLinePosError) / DT;
if (terminalOutput) {
TERMINAL_PRINTF("CURRENT LINE POSITION %9.3f\r\n",
CurrentLinePosition);
TERMINAL_PRINTF("CURRENT LINE POSITION ERROR %9.3f\r\n",
CurrentLinePosError);
}
// SECOND- calculate new servo position
// proportional control term
Pout = KP * CurrentLinePosError;
// integral control term
Iout = KI * SumLinePosError;
// Derivative control term
Dout = KD * DerivError;
if (terminalOutput) {
TERMINAL_PRINTF("KP = %6.4f\r\n", KP);
TERMINAL_PRINTF("KI = %6.4f\r\n", KI);
TERMINAL_PRINTF("KD = %6.4f\r\n", KD);
TERMINAL_PRINTF("Pout = %6.4f\r\n", Pout);
TERMINAL_PRINTF("Iout = %6.4f\r\n", Iout);
TERMINAL_PRINTF("Dout = %6.4f\r\n", Dout);
}
// 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 );
// store for next cycle deriv calculation
LastLinePosError = CurrentLinePosError;
// for tuning control algo only
if (1 == 0)
{
TERMINAL_PRINTF("*** ******************************** \r\n");
TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n",
CurrentLinePosition);
TERMINAL_PRINTF("*** ERROR %9.3f *** \r\n", CurrentLinePosError);
TERMINAL_PRINTF("*** INTEGRAL ERROR %9.3f *** \r\n",
SumLinePosError);
TERMINAL_PRINTF("*** DERIVATIVE ERROR %9.3f *** \r\n", DerivError);
TERMINAL_PRINTF("*** P STEER SETTING %9.3f *** \r\n",
CurrentSteerSetting);
TERMINAL_PRINTF("*** PI STEER SETTING %9.3f *** \r\n",
(CurrentSteerSetting + Iout));
TERMINAL_PRINTF("*** ******************************** \r\n");
wait_ms(1000);
}
}
/****** Steer() ****************************************************************
Purpose: Sets the steering servo output to the value necessary for the current
steering needs. It also checks the value needed to make sure the board isn't
trying to steer harder than the wheels can turn.
Parameters: None
Returns: None
*******************************************************************************/
void Steer()
{
// make sure doesn't go beyond steering limits
if (CurrentSteerSetting > MAX_STEER_RIGHT)
{
CurrentSteerSetting = MAX_STEER_RIGHT;
}
else if(CurrentSteerSetting < MAX_STEER_LEFT)
{
CurrentSteerSetting = MAX_STEER_LEFT;
}
if(terminalOutput)
{
TERMINAL_PRINTF("APPLYING SERVO SETTING %5.3f\r\n",
CurrentSteerSetting);
}
TFC_SetServo(0,CurrentSteerSetting);
}
/****** printDerivLineScanData() ***********************************************
Purpose: This function prints out the line scan derivative data to the terminal
for use in testing and debugging.
Parameters: Takes the array of line scan data derivatives.
Returns: None
*******************************************************************************/
void printDerivLineScanData(float* derivLineScanData)
{
uint32_t i;
uint32_t minCnt = 0;
uint32_t maxCnt = 0;
minCnt = FILTER_ENDS;
maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
TERMINAL_PRINTF("DERIVATIVE DATA:,");
// print one line worth of data (128) from Camera 0
for(i=minCnt;i<maxCnt;i++)
{
TERMINAL_PRINTF("%9.3f",derivLineScanData[i]);
if(i==maxCnt-1) // when last data reached put in line return
TERMINAL_PRINTF("\r\n",derivLineScanData[i]);
else
TERMINAL_PRINTF(", ",derivLineScanData[i]);
}
}
/****** printEdgesFound() ******************************************************
Purpose: This function prints the line edge data to the terminal for line use in
debugging.
Parameters: None
Returns: None
*******************************************************************************/
void printEdgesFound()
{
int i;
// Check that neg edges captured ok
TERMINAL_PRINTF("NEGATIVE EDGES FOUND:,");
for(i=0;i<=numNegEdges-1;i++)
{
TERMINAL_PRINTF("%9.3f",NegEdges[i]);
// when last data reached put in line return
if(i==numNegEdges-1)
TERMINAL_PRINTF("\r\n");
else
TERMINAL_PRINTF(", ");
}
// Check that pos edges captured ok
TERMINAL_PRINTF("POSITIVE EDGES FOUND:,");
for(i=0;i<=numPosEdges-1;i++)
{
TERMINAL_PRINTF("%9.3f",PosEdges[i]);
// when last data reached put in line return
if(i==numPosEdges-1)
TERMINAL_PRINTF("\r\n");
else
TERMINAL_PRINTF(", ");
}
}
/****** PrintOutSomeData() *****************************************************
Purpose: Prints out a selection of Track mode data after image processing.
Parameters: None
Returns: None
*******************************************************************************/
void PrintOutSomeData()
{
printLineScanData(&GrabLineScanImage0[0]);
printDerivLineScanData(&DerivLineScanImage0[0]);
printAdjustLightsData();
printEdgesFound();
}
/****** ActOnTrackStatus() *****************************************************
Purpose: This function decides what to do next based on the current track
status. Its results are printed out to the terminal if terminal output has
been enabled.
Parameters: None
Returns: None
*******************************************************************************/
void ActOnTrackStatus()
{
// Decide what to do next based on current track status
// LINE FOUND!
if (CurrentTrackStatus == LineFound)
{
if (terminalOutput) {
TERMINAL_PRINTF("***************************************** \r\n");
TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n",
CurrentLinePosition);
TERMINAL_PRINTF("***************************************** \r\n");
}
// Update steering position
SteeringControl();
// Apply to servo
Steer();
}
// STARTING GATE FOUND
else if (CurrentTrackStatus == StartGateFound)
{
if (terminalOutput)
{
TERMINAL_PRINTF("***************************************** \r\n");
TERMINAL_PRINTF("********** STARTING GATE FOUND ********** \r\n");
TERMINAL_PRINTF("********** count = %d ********** \r\n",
StartGateFoundCount);
TERMINAL_PRINTF("***************************************** \r\n");
}
// END RACE!
if (startGateStop)
{
if (StartGateFoundCount > STARTGATEFOUNDMAX)
{
go = false; // STOP!!
}
}
}
}
/****** feedbackLights() *******************************************************
Purpose: Turns on board LED's to indicate specific status for debugging
purposes.
Parameters: None
Returns: None
*******************************************************************************/
void feedbackLights()
{
switch (CurrentTrackStatus)
{
case LineFound:
TFC_BAT_LED0_OFF;
TFC_BAT_LED1_ON;
TFC_BAT_LED2_ON;
TFC_BAT_LED3_OFF;
break;
case StartGateFound:
TFC_BAT_LED0_ON;
TFC_BAT_LED1_OFF;
TFC_BAT_LED2_OFF;
TFC_BAT_LED3_ON;
break;
default:
TFC_BAT_LED0_OFF;
TFC_BAT_LED1_OFF;
TFC_BAT_LED2_OFF;
TFC_BAT_LED3_OFF;
}
}
/****** SpeedControl() *********************************************************
Purpose: This function determines the speed settings in the code and set on the
board itself. It then offers multiple ways to apply those settings to the
actual speed output to the wheels.
Its results are printed out to the terminal if terminal output has been
enabled.
Parameters: None
Returns: None
*******************************************************************************/
void SpeedControl()
{
// Get max speed setting from reading pot1 and then adjust
float ErrLimit;
float LeftDriveRatio;
float RightDriveRatio;
// set maximum speed
if (doRisky)
MaxSpeed = LUDICROUS_SPEED * ((TFC_ReadPot(0)+1)/2.0); // faster
else
MaxSpeed = LIGHT_SPEED * ((TFC_ReadPot(0)+1)/2.0); // slower
switch (SPEED_ADJUST)
{
// SPEED ADJUST METHOD 0
// no speed adjust
case 0:
LeftDriveRatio = MAX_POWER;
RightDriveRatio = LeftDriveRatio;
// SPEED ADJUST METHOD 1
// High speed when error is low, low speed when error is high
// lower speed when more than third outside of center
case 1:
ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.33;
if (AbsError > ErrLimit) {
LeftDriveRatio = MIN_POWER;
} else {
LeftDriveRatio = MAX_POWER;
}
RightDriveRatio = LeftDriveRatio;
break;
// SPEED ADJUST METHOD 2
// max/min speed adjusts proportional to absolute value of line error
case 2:
ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO;
LeftDriveRatio =
MAX_POWER - ((MAX_POWER - MIN_POWER) * (AbsError / ErrLimit));
RightDriveRatio = LeftDriveRatio;
break;
// SPEED ADJUST METHOD 3
// wheel relative speed proportional to absolute value of line error
case 3:
// heading right, slow right wheel down
if (CurrentLinePosError > 0)
{
LeftDriveRatio = MAX_POWER;
RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER)
* (CurrentLinePosError * 2 / ( (float) RANGE ) );
}
// heading left, slow left wheel down
else if (CurrentLinePosError < 0)
{
// note sign change due to error being negative
LeftDriveRatio = MAX_POWER - (MIN_POWER - MAX_POWER)
* (CurrentLinePosError * 2 / ( (float) RANGE ) );
RightDriveRatio = MAX_POWER;
}
else
{
LeftDriveRatio = MAX_POWER;
RightDriveRatio = MAX_POWER;
}
break;
case 4:
// SPEED ADJUST METHOD 4
// wheel relative speed proportional to absolute value of line error
// only when above a certain error
ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.1;
// right turn-- slow right wheel down a bit
if (CurrentLinePosError > ErrLimit)
{
LeftDriveRatio = MAX_POWER;
RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER)
* (CurrentLinePosError * 2 / ( (float) RANGE ) );
}
// left turn-- slow left wheel down a bit
else if(CurrentLinePosError < (-1 * ErrLimit))
{
// note sign change due to error being negative
LeftDriveRatio = MAX_POWER - (MIN_POWER - MAX_POWER)
* (CurrentLinePosError * 2 / ( (float) RANGE ) );
RightDriveRatio = MAX_POWER;
}
// when in center drive full speed
else
{
LeftDriveRatio = MAX_POWER;
RightDriveRatio = MAX_POWER;
}
break;
// SPEED ADJUST METHOD 5
// High speed when error is low, low speed when error is high
// lower speed when more than third outside of center
case 5:
ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.2;
if (AbsError > ErrLimit)
{
LeftDriveRatio = MIN_POWER;
}
else
{
LeftDriveRatio = MAX_POWER;
}
RightDriveRatio = LeftDriveRatio;
break;
// SPEED ADJUST METHOD 6
// High speed when error is low, low speed when error is high
// lower speed when more than third outside of center
case 6:
if (AbsError > ABS_ERROR_THRESH)
{
LeftDriveRatio = MIN_POWER;
}
else
{
LeftDriveRatio = MAX_POWER;
}
RightDriveRatio = LeftDriveRatio;
break;
default:
break;
}
// TBD-- add speed adjust based on Xaccel sensor!
// currently no control mechanism as don't have speed sensor
CurrentLeftDriveSetting = (float) (LeftDriveRatio / 100) * MaxSpeed;
CurrentRightDriveSetting = (float) (RightDriveRatio / 100) * MaxSpeed;
if (terminalOutput) {
TERMINAL_PRINTF("Abs Error: %4.2f\r\n", AbsError);
TERMINAL_PRINTF("Error Limit: %4.2f\r\n", ErrLimit);
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);
}
}
/****** Drive() ****************************************************************
Purpose: This is the function that enables the motors to run. It monitors the
pressing of buttons A and B to start and stop the car. It also monitors for
emergency stop conditions and will turn off the motors to stop the car if
necessary.
Parameters: None
Returns: None
*******************************************************************************/
void Drive()
{
// START!
// if not going, go when button A is pressed
if (!go)
{
if(TFC_PUSH_BUTTON_0_PRESSED)
{
go = true;
UnknownCount = 0;
StartGateFoundCount = 0;
startRaceTicker = TFC_Ticker[0]; // keep track of start of race
logDataIndex = 0; // reset log data index
}
}
// STOP!
// if going, stop when button B is pressed
if (go) {
if(TFC_PUSH_BUTTON_1_PRESSED) {
go = false;
StartGateFoundCount = 0;
}
}
// EMERGENCY STOP!
// 'kill switch' to prevent crashes off-track
if (killSwitch)
{
// if track not found after certain time
if (UnknownCount > UNKNOWN_COUNT_MAX)
{
// kill engine
go = false;
StartGateFoundCount = 0;
}
}
// stop!
if (!go)
{
// make sure motors are off
TFC_SetMotorPWM(0,0);
TFC_HBRIDGE_DISABLE;
}
// go!
if (go)
{
TFC_HBRIDGE_ENABLE;
// motor A = right, motor B = left based on way it is mounted
TFC_SetMotorPWM(CurrentRightDriveSetting,CurrentLeftDriveSetting);
}
}
/****** GoSlow() ***************************************************************
Purpose: Sets the doRisky variable to false
Parameters: None
Returns: None
*******************************************************************************/
void GoSlow()
{
doRisky = false;
}
/****** GoFast() ***************************************************************
Purpose: Sets the doRisky variable to true
Parameters: None
Returns: None
*******************************************************************************/
void GoFast()
{
doRisky = true;
}