Jorge Montero / RaceCar

RACE CAR

Dependencies:   FRDM-TFC

Fork of TFC-RACING-DEMO by Daniel Hadad

Spices/Spices.cpp@1:7e3c1d7d50b9, 2014-11-17 (annotated)

Committer:
Jmon14
Date:
Mon Nov 17 20:53:36 2014 +0000
Revision:
1:7e3c1d7d50b9
Parent:
0:98e98e01a6ce 
Commit message

Who changed what in which revision?

UserRevisionLine numberNew contents of line
redxeth 0:98e98e01a6ce 1 #include "mbed.h"
redxeth 0:98e98e01a6ce 2
redxeth 0:98e98e01a6ce 3 #include "Spices.h"
redxeth 0:98e98e01a6ce 4 #include "common.h"
redxeth 0:98e98e01a6ce 5 #include "TFC.h"
redxeth 0:98e98e01a6ce 6
redxeth 0:98e98e01a6ce 7 // camera params
redxeth 0:98e98e01a6ce 8 #define NUM_LINE_SCAN 128
redxeth 0:98e98e01a6ce 9 #define MAX_LINE_SCAN NUM_LINE_SCAN-1
redxeth 0:98e98e01a6ce 10 #define MIN_LINE_WIDTH 0
Jmon14 1:7e3c1d7d50b9 11 #define MAX_LINE_WIDTH 10
redxeth 0:98e98e01a6ce 12 #define FILTER_ENDS 0 // # of pixels at end of camera data to ignore; set to 0 for now, later make 15
redxeth 0:98e98e01a6ce 13 #define RANGE (NUM_LINE_SCAN - (2 * FILTER_ENDS)) // range of camera pixels to consider
redxeth 0:98e98e01a6ce 14 #define ERR_RATIO 0.85 // ratio of max possible error to pixels (have to measure!)
redxeth 0:98e98e01a6ce 15 #define DER_RATIO 0.5 // ratio for der threshold level (was 0.5 initially, may put back)
redxeth 0:98e98e01a6ce 16
redxeth 0:98e98e01a6ce 17 // steer/servo params
Jmon14 1:7e3c1d7d50b9 18 #define MAX_STEER_LEFT -0.21 // value determined by demo mode 1 measure (have to be adjusted with every servo horn attach)
Jmon14 1:7e3c1d7d50b9 19 #define MAX_STEER_RIGHT 0.75 // value determined by demo mode 1 measure
redxeth 0:98e98e01a6ce 20 #define DT 0.02 // # MS of time between intervals (doesn't really matter)
redxeth 0:98e98e01a6ce 21
redxeth 0:98e98e01a6ce 22 // logging parameters
redxeth 0:98e98e01a6ce 23 #define NUM_LOG_FRAMES 700 // # of frames to log (when logging active) ~14 sec worth!
redxeth 0:98e98e01a6ce 24
redxeth 0:98e98e01a6ce 25 // ****** for debug tuning ******
redxeth 0:98e98e01a6ce 26 #define TUNE_SPEED 0.7
redxeth 0:98e98e01a6ce 27 #define TUNE_KP 0.008
redxeth 0:98e98e01a6ce 28 #define TUNE_KI 0
redxeth 0:98e98e01a6ce 29 #define TUNE_KD 0
redxeth 0:98e98e01a6ce 30 #define MIN_POWER 60 // percent min power (estimating for a 2-ft turn => 24" / (24" + 6" car) = 4/5; speed of inner wheel is 20% lower worst case
redxeth 0:98e98e01a6ce 31 #define SPEED_ADJUST 4
redxeth 0:98e98e01a6ce 32 #define ABS_ERROR_THRESH 10 // number of pixels line position offset before changing KP value
redxeth 0:98e98e01a6ce 33 #define CONTROL_METHOD 2 // which control method to use
redxeth 0:98e98e01a6ce 34
redxeth 0:98e98e01a6ce 35
redxeth 0:98e98e01a6ce 36 // Drive/motor params
redxeth 0:98e98e01a6ce 37 // 0.4 way too slow!! need to charge battery
redxeth 0:98e98e01a6ce 38 #define SUB_LIGHT_SPEED 0.5 // moderate speed (value 0 to 1 sent to motors)
redxeth 0:98e98e01a6ce 39 #define LIGHT_SPEED 0.6 // fast...
redxeth 0:98e98e01a6ce 40 #define RIDICULOUS_SPEED 0.7 // faster...
redxeth 0:98e98e01a6ce 41 #define LUDICROUS_SPEED 0.9 // faster still!
redxeth 0:98e98e01a6ce 42 #define MAX_POWER 100 // percent max power (for speed adjustments)
redxeth 0:98e98e01a6ce 43
redxeth 0:98e98e01a6ce 44 // algo params
redxeth 0:98e98e01a6ce 45 #define UNKNOWN_COUNT_MAX 50 // max value to allow for unknown track conditions before killing engine
redxeth 0:98e98e01a6ce 46 #define STARTGATEFOUNDMAX 0 // max value to allow for finding starting gate before killing engine
redxeth 0:98e98e01a6ce 47 #define STARTGATEDELAY 50 // Delay before searching for starting gate to kill engine
redxeth 0:98e98e01a6ce 48
redxeth 0:98e98e01a6ce 49 #define MMA8451_I2C_ADDRESS (0x1d<<1) // address for accelerometer?
redxeth 0:98e98e01a6ce 50
redxeth 0:98e98e01a6ce 51
redxeth 0:98e98e01a6ce 52 /* CAR INTERFACE
redxeth 0:98e98e01a6ce 53
redxeth 0:98e98e01a6ce 54 DIP SWITCH:
redxeth 0:98e98e01a6ce 55 -----------------------------------------------------------------
redxeth 0:98e98e01a6ce 56 1 - ON: Run MCP below; OFF: Run Demo program (see main.cpp)
redxeth 0:98e98e01a6ce 57 2 - ON: Log frame data to array
redxeth 0:98e98e01a6ce 58 3 - ON: Risky race option; OFF: Conservative race option
redxeth 0:98e98e01a6ce 59 4 - ON: Start Gate Kill Switch Active
redxeth 0:98e98e01a6ce 60
redxeth 0:98e98e01a6ce 61 POTS
redxeth 0:98e98e01a6ce 62 -----------------------------------------------------------------
redxeth 0:98e98e01a6ce 63 0 - controls nothing at the moment
redxeth 0:98e98e01a6ce 64 1 - controls nothing at the moment
redxeth 0:98e98e01a6ce 65
redxeth 0:98e98e01a6ce 66 PUSHBUTTONS
redxeth 0:98e98e01a6ce 67 -----------------------------------------------------------------
redxeth 0:98e98e01a6ce 68 A - START car race!
redxeth 0:98e98e01a6ce 69 B - END CAR RACE / (while holding down when 'log frame data' active will also output terminal data)
redxeth 0:98e98e01a6ce 70
redxeth 0:98e98e01a6ce 71 */
redxeth 0:98e98e01a6ce 72
redxeth 0:98e98e01a6ce 73 // LEARNING CAR CLUB 9/10/13:
redxeth 0:98e98e01a6ce 74 // IP need to test-- get around U turns -- more aggressive proportional control? Or derivative? Have camera look farther ahead but not too far ahead
redxeth 0:98e98e01a6ce 75 // IP need to test -- fix lighting for tunnels (if good algo doesn't need lighting!!)
redxeth 0:98e98e01a6ce 76 // -- increase power up to get up hills, brake down hills (accel: http://mbed.org/users/SomeRandomBloke/code/MMA8451Q/#)
redxeth 0:98e98e01a6ce 77 // -- add speed control? (hall effect sensor)
redxeth 0:98e98e01a6ce 78 // DONE -- make sure steering won't go past limits!!
redxeth 0:98e98e01a6ce 79 // 9/12/13 - adjust camera exposure time based on maximum light intensity!!
redxeth 0:98e98e01a6ce 80 // 9/14/13 - DONE -- make derivative threshold related to maximum light intensity!
redxeth 0:98e98e01a6ce 81 // 9/16/13 - crash at car club blew out resistor R8, replaced it and getty twitching when powering servo from USB only. Ok when powering from battery.
redxeth 0:98e98e01a6ce 82 // 9/17/13 - experiments show that derivative control just doesn't work very well-- the tinest error delta causes huge drastic changes, need to use non-linear proportional
redxeth 0:98e98e01a6ce 83 // control instead... parabolic??
redxeth 0:98e98e01a6ce 84 // DONE -- Also need to slow down on curves
redxeth 0:98e98e01a6ce 85 // speed up hills and slow down on downhill...
redxeth 0:98e98e01a6ce 86 // measure speed?
redxeth 0:98e98e01a6ce 87 // 9/18/13 - Test track work: doesn't appear to be sampling camera fast enough-- not able to handle the wiggly track and sometimes not able to handle the curves!
redxeth 0:98e98e01a6ce 88 // TODO: Need to increase rate at which camera sampled and decisions are made!! Look to codewarrior code
redxeth 0:98e98e01a6ce 89 // Need to cut off left/right ends of camera data-- seems to not read line properly in well lit rooms
redxeth 0:98e98e01a6ce 90 // DONE Speed adjust as you go round; Need to have it slow down "into" curve, speed up again "out of curve"
redxeth 0:98e98e01a6ce 91 // 9/23/13 - Definitely not processing camera fast enough-- seems to not react well when speed up the car. Goes slow just fine all the way 'round.
redxeth 0:98e98e01a6ce 92 // TODO: Need to see how often TFC_LineScanImageReady gets updated. If update camera sample freq how will that impact exposure? Light adjustment algos should
redxeth 0:98e98e01a6ce 93 // be able to handle it. Need to be able to measure the 'processing time' required. Also wondering if 20mS is fast enough for updating the servo?? Need
redxeth 0:98e98e01a6ce 94 // to calculate how fast servo needs to really go based on track curves and speed.
redxeth 0:98e98e01a6ce 95 // TODO: Need a way to measure speed I think as well.
redxeth 0:98e98e01a6ce 96 // TODO: Need to control speed differentially across the different motors!! See Eli video!!
redxeth 0:98e98e01a6ce 97 // TODO: Use this to get excel feedback quicker: http://strokescribe.com/en/serial-port-download.html (Doesn't work too well-- prefer my own excel method)
redxeth 0:98e98e01a6ce 98 // TODO: Use PID control for steering!
redxeth 0:98e98e01a6ce 99 // TODO: Use Speed control (PID?) --- add speed sensor!
redxeth 0:98e98e01a6ce 100 // 10/8/13 Reduced speed control to 90% (was 85%)-- seems to go off track and lose line at high speed-- mainly on the U turns
redxeth 0:98e98e01a6ce 101 // Need to figure out why derivative control in steering doesn't work well. Add integral control.
redxeth 0:98e98e01a6ce 102 // Latest track times (no hill, no bumps, no tunnel, only squiggles) = 11.8sec at high speed with speed control
redxeth 0:98e98e01a6ce 103 // Losing time on the curves-- need to optimize!!
redxeth 0:98e98e01a6ce 104 // Worry about hill later-- need to get track times down around 8sec first.
redxeth 0:98e98e01a6ce 105 // *****************************
redxeth 0:98e98e01a6ce 106 // ** Implement some method to acount for U-TURNS on track-- and to help even go beyond what camera can see as far as estimating line position
redxeth 0:98e98e01a6ce 107 // ** (U-TURNS take the longest time out of track)
redxeth 0:98e98e01a6ce 108 // ** Method to help find the line even when not really visible
redxeth 0:98e98e01a6ce 109 // *****************************
redxeth 0:98e98e01a6ce 110 // 10/17/13
redxeth 0:98e98e01a6ce 111 // NEED CURVE AND WIGGLE DETECT!!
redxeth 0:98e98e01a6ce 112 // 10/20/13 Added logging capability and 'algo time' detect
redxeth 0:98e98e01a6ce 113 // It is not finding the line on the edges when on a curve-- likely because my line detect algo requires both edges
redxeth 0:98e98e01a6ce 114 // TODO-- need method that can use only one edge!
redxeth 0:98e98e01a6ce 115
redxeth 0:98e98e01a6ce 116 // TODO LIST
redxeth 0:98e98e01a6ce 117 // - Speed Control via Sensor WAITING ON SENSOR
redxeth 0:98e98e01a6ce 118 // - Differential drive around curves DONE -- still trying to figure out what %age to drop on turns
redxeth 0:98e98e01a6ce 119 // - Full PID steering control IN PROGRESS
redxeth 0:98e98e01a6ce 120 // - Starting gate kill engine debug IN PROGRESS -- need to add delay
redxeth 0:98e98e01a6ce 121 // - Off track kill engine debug -- IN PROGRESS-- seems to kill car prematurely
redxeth 0:98e98e01a6ce 122
redxeth 0:98e98e01a6ce 123 // image processing vars
redxeth 0:98e98e01a6ce 124 uint16_t GrabLineScanImage0[NUM_LINE_SCAN]; // snapshot of camera data for this 'frame'
redxeth 0:98e98e01a6ce 125 float DerivLineScanImage0[NUM_LINE_SCAN]; // derivative of line scan data
redxeth 0:98e98e01a6ce 126 float NegEdges[NUM_LINE_SCAN]; // array-- set of where in line scan data negative edges found
redxeth 0:98e98e01a6ce 127 float PosEdges[NUM_LINE_SCAN]; // array-- set of where in line scan data positive edges found
redxeth 0:98e98e01a6ce 128 uint16_t numNegEdges = 0, numPosEdges = 0; // max value of valid neg and positive indices (also serves as a count of # edges found)
redxeth 0:98e98e01a6ce 129 uint16_t MaxLightIntensity = 0; // max measured light intensity -- to account for lighting differences
redxeth 0:98e98e01a6ce 130 uint16_t MinLightIntensity = (1 << 12); // min measured light intensity -- to account for lighting differences
redxeth 0:98e98e01a6ce 131 float maxDerVal = 0; // max deriv value
redxeth 0:98e98e01a6ce 132 float minDerVal = (float) (1 << 12); // min deriv value
redxeth 0:98e98e01a6ce 133 float aveDerVal = 0; // average deriv value
redxeth 0:98e98e01a6ce 134 float DerivThreshold = (1 << 9); // Derivative Threshold (default)
redxeth 0:98e98e01a6ce 135 float PosDerivThreshold = (1 << 9); // Pos Edge Derivative Threshold (default)
redxeth 0:98e98e01a6ce 136 float NegDerivThreshold = (1 << 9); // Neg Edge Derivative Threshold (default)
redxeth 0:98e98e01a6ce 137
redxeth 0:98e98e01a6ce 138
redxeth 0:98e98e01a6ce 139 // Steering control variables
Jmon14 1:7e3c1d7d50b9 140 float CurrentLinePosition1; // Current left position of track line (in pixels -- 0 to 127)
Jmon14 1:7e3c1d7d50b9 141 float CurrentLinePosition2; // Current right position of track line (in pixels -- 0 to 127)
Jmon14 1:7e3c1d7d50b9 142 float CurrentLinePosition; // Current central position of track line (in pixels -- 0 to 127)
Jmon14 1:7e3c1d7d50b9 143 float TrackWidth = 100;
redxeth 0:98e98e01a6ce 144 float LastLinePosition; // Last position of track line (in pixels -- 0 to 127)
redxeth 0:98e98e01a6ce 145 float CurrentLinePosError = 0; // Current line position error (used for derivative calc)
redxeth 0:98e98e01a6ce 146 float AbsError;
redxeth 0:98e98e01a6ce 147 float LastLinePosError = 0; // Last line position error (used for derivative calc)
redxeth 0:98e98e01a6ce 148 float SumLinePosError = 0; // Sum of line position error (used for integral calc)
redxeth 0:98e98e01a6ce 149 float DerivError = 0; // Derivative of error
redxeth 0:98e98e01a6ce 150 float CurrentSteerSetting = (MAX_STEER_RIGHT + MAX_STEER_LEFT) / 2; // drive straight at first
redxeth 0:98e98e01a6ce 151 float CurrentLeftDriveSetting = 0; // Drive setting (left wheel)
redxeth 0:98e98e01a6ce 152 float CurrentRightDriveSetting = 0; // Drive setting (right wheel)
redxeth 0:98e98e01a6ce 153
redxeth 0:98e98e01a6ce 154 // Speed control vars
redxeth 0:98e98e01a6ce 155 float MaxSpeed; // maximum speed allowed
redxeth 0:98e98e01a6ce 156
redxeth 0:98e98e01a6ce 157 uint16_t startRaceTicker; // ticker at start of race1
redxeth 0:98e98e01a6ce 158
redxeth 0:98e98e01a6ce 159 // Custom Data Types
redxeth 0:98e98e01a6ce 160 typedef enum TrackStatusType {Unknown,
redxeth 0:98e98e01a6ce 161 LineFound,
redxeth 0:98e98e01a6ce 162 StartGateFound,
redxeth 0:98e98e01a6ce 163 LineJustLeft} TrackStatusType;
redxeth 0:98e98e01a6ce 164
redxeth 0:98e98e01a6ce 165 TrackStatusType CurrentTrackStatus; // current track status
redxeth 0:98e98e01a6ce 166 TrackStatusType LastTrackStatus; // last track status
redxeth 0:98e98e01a6ce 167
redxeth 0:98e98e01a6ce 168 /* typedef enum TrackType {NotSure,
redxeth 0:98e98e01a6ce 169 Straight,
redxeth 0:98e98e01a6ce 170 Curve,
redxeth 0:98e98e01a6ce 171 Wiggle,
redxeth 0:98e98e01a6ce 172 Bumps,
redxeth 0:98e98e01a6ce 173 StartGate,
redxeth 0:98e98e01a6ce 174 UpHill,
redxeth 0:98e98e01a6ce 175 DownHill} TrackType;
redxeth 0:98e98e01a6ce 176
redxeth 0:98e98e01a6ce 177 TrackType CurrentTrack; */
redxeth 0:98e98e01a6ce 178
redxeth 0:98e98e01a6ce 179
redxeth 0:98e98e01a6ce 180 struct LogData {
redxeth 0:98e98e01a6ce 181 float linepos;
redxeth 0:98e98e01a6ce 182 float steersetting;
redxeth 0:98e98e01a6ce 183 float leftdrivesetting;
redxeth 0:98e98e01a6ce 184 float rightdrivesetting;
redxeth 0:98e98e01a6ce 185 };
redxeth 0:98e98e01a6ce 186
redxeth 0:98e98e01a6ce 187 LogData frameLogs[NUM_LOG_FRAMES]; // array of log data to store
redxeth 0:98e98e01a6ce 188 int logDataIndex; // index for log data
redxeth 0:98e98e01a6ce 189
redxeth 0:98e98e01a6ce 190
redxeth 0:98e98e01a6ce 191 int StartGateFoundCount = 0; // how many times start gate has been found
redxeth 0:98e98e01a6ce 192 int UnknownCount = 0; // how many times nothing has been found (to help with kill switch implementation)
redxeth 0:98e98e01a6ce 193 bool go = false; // Car can go! Should be set to false to start.
redxeth 0:98e98e01a6ce 194
redxeth 0:98e98e01a6ce 195 // EXTRA CONTROL PARAMETERS
redxeth 0:98e98e01a6ce 196 bool debugFakeMode = false; // if true, ignores real camera and uses fake camera input instead; used for data processing debug
redxeth 0:98e98e01a6ce 197 int terminalOutput = 0; // set debug level for terminal output
redxeth 0:98e98e01a6ce 198 // 0 : no terminal output, race!
redxeth 0:98e98e01a6ce 199 // 1 : output just to measure frame rate
redxeth 0:98e98e01a6ce 200 // 2 : output for measuring time of operations
redxeth 0:98e98e01a6ce 201 // 3 : output with delay
redxeth 0:98e98e01a6ce 202 bool doLogData = false; // whether to capture log data to output later on
redxeth 0:98e98e01a6ce 203 bool killSwitch = false; // whether to enable Kill Switch (allow engine to stop after not finding track)
redxeth 0:98e98e01a6ce 204 bool startGateStop = false; // whether to stop or not depending on starting gate reading
redxeth 0:98e98e01a6ce 205 bool doRisky = false; // race style-- whether conservative or risky
redxeth 0:98e98e01a6ce 206
redxeth 0:98e98e01a6ce 207 // timer stuff
redxeth 0:98e98e01a6ce 208 Timer timer;
redxeth 0:98e98e01a6ce 209 int after_time, before_time, start_time, last_start_time;
redxeth 0:98e98e01a6ce 210 bool run_once = false;
redxeth 0:98e98e01a6ce 211
redxeth 0:98e98e01a6ce 212 void MasterControlProgram()
redxeth 0:98e98e01a6ce 213 {
redxeth 0:98e98e01a6ce 214
redxeth 0:98e98e01a6ce 215 // put here all things want to run only once after reset
redxeth 0:98e98e01a6ce 216 if (!run_once){
redxeth 0:98e98e01a6ce 217 if ((terminalOutput == 1) || (terminalOutput == 2)){
redxeth 0:98e98e01a6ce 218 timer.start();
redxeth 0:98e98e01a6ce 219 }
redxeth 0:98e98e01a6ce 220 run_once = true;
redxeth 0:98e98e01a6ce 221 }
redxeth 0:98e98e01a6ce 222
redxeth 0:98e98e01a6ce 223 // read DIP switches and Pots for data
redxeth 0:98e98e01a6ce 224 readSwitches();
redxeth 0:98e98e01a6ce 225
redxeth 0:98e98e01a6ce 226 // Every 4s (or Terminal Output is off, i.e. race mode!)
redxeth 0:98e98e01a6ce 227 // AND line scan image ready (or fake mode where image is always ready)
redxeth 0:98e98e01a6ce 228 // (ticker updates every 2ms) (Line scan image ready very 20mS?)
Jmon14 1:7e3c1d7d50b9 229 if((TFC_Ticker[0]>5000 || (terminalOutput != 3)) && (TFC_LineScanImageReady>0 || debugFakeMode))
redxeth 0:98e98e01a6ce 230 {
redxeth 0:98e98e01a6ce 231
redxeth 0:98e98e01a6ce 232 // stuff that needs to be reset with each image frame
redxeth 0:98e98e01a6ce 233 if (terminalOutput == 1) {
redxeth 0:98e98e01a6ce 234 last_start_time = start_time;
redxeth 0:98e98e01a6ce 235 start_time = timer.read_us();
redxeth 0:98e98e01a6ce 236 TERMINAL_PRINTF("TIME:Between frames:%d:uSec\r\n", start_time - last_start_time);
redxeth 0:98e98e01a6ce 237 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 238 }
redxeth 0:98e98e01a6ce 239 TFC_Ticker[0] = 0;
redxeth 0:98e98e01a6ce 240 TFC_LineScanImageReady=0; // must reset to 0 after detecting non-zero
redxeth 0:98e98e01a6ce 241 MaxLightIntensity = 0; // reset
redxeth 0:98e98e01a6ce 242 MinLightIntensity = (1 << 12); // reset
redxeth 0:98e98e01a6ce 243
redxeth 0:98e98e01a6ce 244 // grab camera frame
redxeth 0:98e98e01a6ce 245 grabCameraFrame();
redxeth 0:98e98e01a6ce 246
redxeth 0:98e98e01a6ce 247 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 248 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 249 TERMINAL_PRINTF("TIME:TO AFTER grabCameraFrame:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 250 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 251 }
redxeth 0:98e98e01a6ce 252
redxeth 0:98e98e01a6ce 253 // calcalate derivative of linescandata, filter starttime data
redxeth 0:98e98e01a6ce 254 derivativeLineScan(&GrabLineScanImage0[0], &DerivLineScanImage0[0]);
redxeth 0:98e98e01a6ce 255
redxeth 0:98e98e01a6ce 256 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 257 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 258 TERMINAL_PRINTF("TIME:TO AFTER derivativeLineScan:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 259 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 260 }
redxeth 0:98e98e01a6ce 261
redxeth 0:98e98e01a6ce 262 // adjust deriv threshold based on max lighting value
redxeth 0:98e98e01a6ce 263 // has to be called before find edges
redxeth 0:98e98e01a6ce 264 adjustLights();
redxeth 0:98e98e01a6ce 265
redxeth 0:98e98e01a6ce 266 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 267 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 268 TERMINAL_PRINTF("TIME:TO AFTER adjustLights:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 269 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 270 }
redxeth 0:98e98e01a6ce 271
redxeth 0:98e98e01a6ce 272 //find edges from derivative data
redxeth 0:98e98e01a6ce 273 findEdges_v2(&DerivLineScanImage0[0]);
redxeth 0:98e98e01a6ce 274
redxeth 0:98e98e01a6ce 275 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 276 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 277 TERMINAL_PRINTF("TIME:TO AFTER findEdges_v2:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 278 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 279 }
redxeth 0:98e98e01a6ce 280
redxeth 0:98e98e01a6ce 281 // turn on terminal output if line not found -- FOR DEBUG
redxeth 0:98e98e01a6ce 282 //if (CurrentTrackStatus == Unknown)
redxeth 0:98e98e01a6ce 283 // terminalOutput = 1;
redxeth 0:98e98e01a6ce 284
redxeth 0:98e98e01a6ce 285 //review edge data and set position or starting flag appropriately
redxeth 0:98e98e01a6ce 286 reviewEdges();
redxeth 0:98e98e01a6ce 287
redxeth 0:98e98e01a6ce 288 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 289 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 290 TERMINAL_PRINTF("TIME:TO AFTER reviewEdges:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 291 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 292 }
redxeth 0:98e98e01a6ce 293
redxeth 0:98e98e01a6ce 294 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 295 // print data to Terminal for camera 0
redxeth 0:98e98e01a6ce 296 printLineScanData(&GrabLineScanImage0[0]);
redxeth 0:98e98e01a6ce 297
redxeth 0:98e98e01a6ce 298 // print deriviative of linescandata, filter starttime data
redxeth 0:98e98e01a6ce 299 printDerivLineScanData(&DerivLineScanImage0[0]);
redxeth 0:98e98e01a6ce 300
redxeth 0:98e98e01a6ce 301 printAdjustLightsData();
redxeth 0:98e98e01a6ce 302
redxeth 0:98e98e01a6ce 303 printEdgesFound();
redxeth 0:98e98e01a6ce 304
redxeth 0:98e98e01a6ce 305 }
redxeth 0:98e98e01a6ce 306
redxeth 0:98e98e01a6ce 307 // ** Track Status available at this point **
redxeth 0:98e98e01a6ce 308
redxeth 0:98e98e01a6ce 309 // Update things based on latest track status
redxeth 0:98e98e01a6ce 310 // e.g. change steering setting, stop car, ...
Jmon14 1:7e3c1d7d50b9 311
redxeth 0:98e98e01a6ce 312 ActOnTrackStatus();
redxeth 0:98e98e01a6ce 313
redxeth 0:98e98e01a6ce 314 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 315 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 316 TERMINAL_PRINTF("TIME:TO AFTER ActOnTrackStatus:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 317 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 318 }
Jmon14 1:7e3c1d7d50b9 319
redxeth 0:98e98e01a6ce 320 //give LED feedback as to track status
redxeth 0:98e98e01a6ce 321 feedbackLights();
redxeth 0:98e98e01a6ce 322
redxeth 0:98e98e01a6ce 323 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 324 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 325 TERMINAL_PRINTF("TIME:TO AFTER feedbackLights:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 326 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 327 }
redxeth 0:98e98e01a6ce 328
redxeth 0:98e98e01a6ce 329 // control max power (speed)
redxeth 0:98e98e01a6ce 330 SpeedControl();
redxeth 0:98e98e01a6ce 331
redxeth 0:98e98e01a6ce 332 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 333 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 334 TERMINAL_PRINTF("TIME:TO AFTER SpeedControl:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 335 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 336 }
redxeth 0:98e98e01a6ce 337
redxeth 0:98e98e01a6ce 338 // Drive!!
redxeth 0:98e98e01a6ce 339 Drive();
redxeth 0:98e98e01a6ce 340
redxeth 0:98e98e01a6ce 341 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 342 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 343 TERMINAL_PRINTF("TIME:TO AFTER Drive:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 344 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 345 }
redxeth 0:98e98e01a6ce 346
redxeth 0:98e98e01a6ce 347 // wait_ms(1);
redxeth 0:98e98e01a6ce 348
redxeth 0:98e98e01a6ce 349 // Capture Log data while driving
redxeth 0:98e98e01a6ce 350 if (go && doLogData) {
redxeth 0:98e98e01a6ce 351 captureData();
redxeth 0:98e98e01a6ce 352 }
redxeth 0:98e98e01a6ce 353
redxeth 0:98e98e01a6ce 354 // Dump Log data to Terminal while stopped and holding B button
redxeth 0:98e98e01a6ce 355 if (!go && doLogData && TFC_PUSH_BUTTON_1_PRESSED) {
redxeth 0:98e98e01a6ce 356 dumpData();
redxeth 0:98e98e01a6ce 357 }
redxeth 0:98e98e01a6ce 358
redxeth 0:98e98e01a6ce 359 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 360 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 361 TERMINAL_PRINTF("TIME: ENTIRE FRAME (include prints):%d:uSec\r\n", after_time - start_time);
redxeth 0:98e98e01a6ce 362 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 363 }
redxeth 0:98e98e01a6ce 364
redxeth 0:98e98e01a6ce 365 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 366 TERMINAL_PRINTF("\r\n**************************END********************************\r\n");
redxeth 0:98e98e01a6ce 367 }
redxeth 0:98e98e01a6ce 368
redxeth 0:98e98e01a6ce 369 }
redxeth 0:98e98e01a6ce 370 }
redxeth 0:98e98e01a6ce 371
redxeth 0:98e98e01a6ce 372 void dumpData()
redxeth 0:98e98e01a6ce 373 {
redxeth 0:98e98e01a6ce 374 TERMINAL_PRINTF("INDEX,LINEPOS,STEERSETTING,LEFTDRIVESETTING,RIGHTDRIVESETTING\r\n");
redxeth 0:98e98e01a6ce 375 for(logDataIndex=0;logDataIndex<NUM_LOG_FRAMES;logDataIndex++) {
redxeth 0:98e98e01a6ce 376 TERMINAL_PRINTF("%d,%6.2f,%6.2f,%6.2f,%6.2f\r\n",logDataIndex,frameLogs[logDataIndex].linepos,frameLogs[logDataIndex].steersetting,frameLogs[logDataIndex].leftdrivesetting,frameLogs[logDataIndex].rightdrivesetting);
redxeth 0:98e98e01a6ce 377 }
redxeth 0:98e98e01a6ce 378 }
redxeth 0:98e98e01a6ce 379
redxeth 0:98e98e01a6ce 380 void captureData()
redxeth 0:98e98e01a6ce 381 {
redxeth 0:98e98e01a6ce 382 frameLogs[logDataIndex].linepos = CurrentLinePosition;
redxeth 0:98e98e01a6ce 383 frameLogs[logDataIndex].steersetting = CurrentSteerSetting;
redxeth 0:98e98e01a6ce 384 frameLogs[logDataIndex].leftdrivesetting = CurrentLeftDriveSetting;
redxeth 0:98e98e01a6ce 385 frameLogs[logDataIndex].rightdrivesetting = CurrentRightDriveSetting;
redxeth 0:98e98e01a6ce 386
redxeth 0:98e98e01a6ce 387 // increment index
redxeth 0:98e98e01a6ce 388 logDataIndex++;
redxeth 0:98e98e01a6ce 389 if (logDataIndex > NUM_LOG_FRAMES)
redxeth 0:98e98e01a6ce 390 logDataIndex = 0;
redxeth 0:98e98e01a6ce 391 }
redxeth 0:98e98e01a6ce 392
redxeth 0:98e98e01a6ce 393 void readSwitches()
redxeth 0:98e98e01a6ce 394 {
redxeth 0:98e98e01a6ce 395
redxeth 0:98e98e01a6ce 396 // ********* GATHER DIP SWITCH INPUTS *********
redxeth 0:98e98e01a6ce 397 if(TFC_GetDIP_Switch()&0x02) // SWITCH 2
redxeth 0:98e98e01a6ce 398 doLogData = true; // Log data to array
redxeth 0:98e98e01a6ce 399 else
redxeth 0:98e98e01a6ce 400 doLogData = false; // normal operation
redxeth 0:98e98e01a6ce 401
redxeth 0:98e98e01a6ce 402 if(TFC_GetDIP_Switch()&0x04) // SWITCH 3
redxeth 0:98e98e01a6ce 403 doRisky = true;
redxeth 0:98e98e01a6ce 404 else
redxeth 0:98e98e01a6ce 405 doRisky = false;
redxeth 0:98e98e01a6ce 406
redxeth 0:98e98e01a6ce 407 if(TFC_GetDIP_Switch()&0x08) // SWITCH 4 control start stop gate
redxeth 0:98e98e01a6ce 408 startGateStop = true;
redxeth 0:98e98e01a6ce 409 else
redxeth 0:98e98e01a6ce 410 startGateStop = false;
redxeth 0:98e98e01a6ce 411
redxeth 0:98e98e01a6ce 412
redxeth 0:98e98e01a6ce 413 }
redxeth 0:98e98e01a6ce 414
redxeth 0:98e98e01a6ce 415 void grabCameraFrame()
redxeth 0:98e98e01a6ce 416 {
redxeth 0:98e98e01a6ce 417 uint32_t i = 0;
redxeth 0:98e98e01a6ce 418 uint8_t fake_type = 4; // type of fake data if used
redxeth 0:98e98e01a6ce 419
redxeth 0:98e98e01a6ce 420 for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data (128) from Camera 0
redxeth 0:98e98e01a6ce 421 {
redxeth 0:98e98e01a6ce 422
redxeth 0:98e98e01a6ce 423 if (debugFakeMode) { // use fake camera data
redxeth 0:98e98e01a6ce 424 switch (fake_type) {
redxeth 0:98e98e01a6ce 425 case 0: // ideal track -- line in center
redxeth 0:98e98e01a6ce 426 if (i<57 || i > 70)
redxeth 0:98e98e01a6ce 427 GrabLineScanImage0[i] = 0xFFF; // no line
redxeth 0:98e98e01a6ce 428 else
redxeth 0:98e98e01a6ce 429 GrabLineScanImage0[i] = 0x4B0; // line
redxeth 0:98e98e01a6ce 430 break;
redxeth 0:98e98e01a6ce 431 case 1: // ideal track -- line to the left
redxeth 0:98e98e01a6ce 432 if (i<27 || i > 40)
redxeth 0:98e98e01a6ce 433 GrabLineScanImage0[i] = 0xFFF; // no line
redxeth 0:98e98e01a6ce 434 else
redxeth 0:98e98e01a6ce 435 GrabLineScanImage0[i] = 0x4B0; // line
redxeth 0:98e98e01a6ce 436 break;
redxeth 0:98e98e01a6ce 437 case 2: // ideal track -- line to the right
redxeth 0:98e98e01a6ce 438 if (i<87 || i > 100)
redxeth 0:98e98e01a6ce 439 GrabLineScanImage0[i] = 0xFFF; // no line
redxeth 0:98e98e01a6ce 440 else
redxeth 0:98e98e01a6ce 441 GrabLineScanImage0[i] = 0x4B0; // line
redxeth 0:98e98e01a6ce 442 break;
redxeth 0:98e98e01a6ce 443 case 3: // ideal track -- starting gate!
redxeth 0:98e98e01a6ce 444 // TBD
redxeth 0:98e98e01a6ce 445 break;
redxeth 0:98e98e01a6ce 446 case 4: // less than ideal track -- debug multi-edge issue!
redxeth 0:98e98e01a6ce 447 if (i<54)
redxeth 0:98e98e01a6ce 448 GrabLineScanImage0[i] = 4000; // no line
redxeth 0:98e98e01a6ce 449 if (i == 54)
redxeth 0:98e98e01a6ce 450 GrabLineScanImage0[i] = 3370; // neg edge
redxeth 0:98e98e01a6ce 451 if (i == 55)
redxeth 0:98e98e01a6ce 452 GrabLineScanImage0[i] = 3309; // neg edge
redxeth 0:98e98e01a6ce 453 if (i == 56)
redxeth 0:98e98e01a6ce 454 GrabLineScanImage0[i] = 2016; // neg edge
redxeth 0:98e98e01a6ce 455 if (i == 57)
redxeth 0:98e98e01a6ce 456 GrabLineScanImage0[i] = 711; // neg edge
redxeth 0:98e98e01a6ce 457 if (i == 58)
redxeth 0:98e98e01a6ce 458 GrabLineScanImage0[i] = 696; // neg edge
redxeth 0:98e98e01a6ce 459 if ((i>58) && (i<69))
redxeth 0:98e98e01a6ce 460 GrabLineScanImage0[i] = 500; // line
redxeth 0:98e98e01a6ce 461 if (i == 69)
redxeth 0:98e98e01a6ce 462 GrabLineScanImage0[i] = 1800; // pos edge
redxeth 0:98e98e01a6ce 463 if (i > 69)
redxeth 0:98e98e01a6ce 464 GrabLineScanImage0[i] = 4000; // no line
redxeth 0:98e98e01a6ce 465 default:
redxeth 0:98e98e01a6ce 466 break;
redxeth 0:98e98e01a6ce 467 }
redxeth 0:98e98e01a6ce 468
redxeth 0:98e98e01a6ce 469 } else { // use real camera data
redxeth 0:98e98e01a6ce 470 GrabLineScanImage0[i] = TFC_LineScanImage0[i];
redxeth 0:98e98e01a6ce 471 }
redxeth 0:98e98e01a6ce 472 }
redxeth 0:98e98e01a6ce 473
redxeth 0:98e98e01a6ce 474
redxeth 0:98e98e01a6ce 475 }
redxeth 0:98e98e01a6ce 476
redxeth 0:98e98e01a6ce 477 void printLineScanData(uint16_t* LineScanData)
redxeth 0:98e98e01a6ce 478 {
redxeth 0:98e98e01a6ce 479 uint32_t i = 0;
redxeth 0:98e98e01a6ce 480 float Val;
redxeth 0:98e98e01a6ce 481
redxeth 0:98e98e01a6ce 482 TERMINAL_PRINTF("LINE SCAN DATA:,");
redxeth 0:98e98e01a6ce 483
redxeth 0:98e98e01a6ce 484 for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data (128) from Camera 0
redxeth 0:98e98e01a6ce 485 {
redxeth 0:98e98e01a6ce 486 if (1 == 1) { // use float to print
redxeth 0:98e98e01a6ce 487 Val = (float) LineScanData[i];
redxeth 0:98e98e01a6ce 488 TERMINAL_PRINTF("%9.3f",Val);
redxeth 0:98e98e01a6ce 489 if(i==MAX_LINE_SCAN) // when last data reached put in line return
redxeth 0:98e98e01a6ce 490 TERMINAL_PRINTF("\r\n");
redxeth 0:98e98e01a6ce 491 else
redxeth 0:98e98e01a6ce 492 TERMINAL_PRINTF(",");
redxeth 0:98e98e01a6ce 493 } else {
redxeth 0:98e98e01a6ce 494 TERMINAL_PRINTF("0x%X",LineScanData[i]);
redxeth 0:98e98e01a6ce 495 if(i==MAX_LINE_SCAN) // when last data reached put in line return
redxeth 0:98e98e01a6ce 496 TERMINAL_PRINTF("\r\n",LineScanData[i]);
redxeth 0:98e98e01a6ce 497 else
redxeth 0:98e98e01a6ce 498 TERMINAL_PRINTF(",",LineScanData[i]);
redxeth 0:98e98e01a6ce 499 }
redxeth 0:98e98e01a6ce 500 }
redxeth 0:98e98e01a6ce 501
redxeth 0:98e98e01a6ce 502 }
redxeth 0:98e98e01a6ce 503
redxeth 0:98e98e01a6ce 504 void printDerivLineScanData(float* derivLineScanData)
redxeth 0:98e98e01a6ce 505 {
redxeth 0:98e98e01a6ce 506 uint32_t i, minCnt = 0, maxCnt = 0;
redxeth 0:98e98e01a6ce 507
redxeth 0:98e98e01a6ce 508 minCnt = FILTER_ENDS;
redxeth 0:98e98e01a6ce 509 maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
redxeth 0:98e98e01a6ce 510
redxeth 0:98e98e01a6ce 511 TERMINAL_PRINTF("DERIVATIVE DATA:,");
redxeth 0:98e98e01a6ce 512
redxeth 0:98e98e01a6ce 513 for(i=minCnt;i<maxCnt;i++) // print one line worth of data (128) from Camera 0
redxeth 0:98e98e01a6ce 514 {
redxeth 0:98e98e01a6ce 515 TERMINAL_PRINTF("%9.3f",derivLineScanData[i]);
redxeth 0:98e98e01a6ce 516 if(i==maxCnt-1) // when last data reached put in line return
redxeth 0:98e98e01a6ce 517 TERMINAL_PRINTF("\r\n",derivLineScanData[i]);
redxeth 0:98e98e01a6ce 518 else
redxeth 0:98e98e01a6ce 519 TERMINAL_PRINTF(", ",derivLineScanData[i]);
redxeth 0:98e98e01a6ce 520 }
redxeth 0:98e98e01a6ce 521
redxeth 0:98e98e01a6ce 522 }
redxeth 0:98e98e01a6ce 523
redxeth 0:98e98e01a6ce 524 void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut)
redxeth 0:98e98e01a6ce 525 {
redxeth 0:98e98e01a6ce 526
redxeth 0:98e98e01a6ce 527 uint32_t i, minCnt = 0, maxCnt = 0;
redxeth 0:98e98e01a6ce 528 float DerVal, upperDerVal, lowerDerVal = 0;
redxeth 0:98e98e01a6ce 529
redxeth 0:98e98e01a6ce 530 maxDerVal = 0;
redxeth 0:98e98e01a6ce 531 minDerVal = (float) (1 << 12);
redxeth 0:98e98e01a6ce 532 aveDerVal = 0;
redxeth 0:98e98e01a6ce 533
redxeth 0:98e98e01a6ce 534 minCnt = FILTER_ENDS;
redxeth 0:98e98e01a6ce 535 maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
redxeth 0:98e98e01a6ce 536
redxeth 0:98e98e01a6ce 537 // TERMINAL_PRINTF("i, upperDerVal, lowerDerVal, DerVal\r\n");
redxeth 0:98e98e01a6ce 538
redxeth 0:98e98e01a6ce 539 for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0
redxeth 0:98e98e01a6ce 540 {
redxeth 0:98e98e01a6ce 541
redxeth 0:98e98e01a6ce 542 // store max light intensity value
redxeth 0:98e98e01a6ce 543 if (LineScanDataIn[i] > MaxLightIntensity)
redxeth 0:98e98e01a6ce 544 MaxLightIntensity = LineScanDataIn[i];
redxeth 0:98e98e01a6ce 545
redxeth 0:98e98e01a6ce 546 // store min light intensity value
redxeth 0:98e98e01a6ce 547 if (LineScanDataIn[i] < MinLightIntensity)
redxeth 0:98e98e01a6ce 548 MinLightIntensity = LineScanDataIn[i];
redxeth 0:98e98e01a6ce 549
redxeth 0:98e98e01a6ce 550
redxeth 0:98e98e01a6ce 551 // Central Derivative
redxeth 0:98e98e01a6ce 552 if (i==minCnt) { // start point
redxeth 0:98e98e01a6ce 553 upperDerVal = (float)(LineScanDataIn[i+1]);
redxeth 0:98e98e01a6ce 554 lowerDerVal = (float)(LineScanDataIn[i]); // make same as start point
redxeth 0:98e98e01a6ce 555 } else if (i==maxCnt - 1){ // end point
redxeth 0:98e98e01a6ce 556 upperDerVal = (float)(LineScanDataIn[i]); // make same as end point
redxeth 0:98e98e01a6ce 557 lowerDerVal = (float)(LineScanDataIn[i-1]);
redxeth 0:98e98e01a6ce 558 } else { // any other point
redxeth 0:98e98e01a6ce 559 upperDerVal = (float)(LineScanDataIn[i+1]);
redxeth 0:98e98e01a6ce 560 lowerDerVal = (float)(LineScanDataIn[i-1]);
redxeth 0:98e98e01a6ce 561 }
redxeth 0:98e98e01a6ce 562 DerVal = (upperDerVal - lowerDerVal) / 2;
redxeth 0:98e98e01a6ce 563 // TERMINAL_PRINTF("%d,%9.3f,%9.3f,%9.3f\r\n", i, upperDerVal, lowerDerVal, DerVal);
redxeth 0:98e98e01a6ce 564
redxeth 0:98e98e01a6ce 565 if (DerVal > maxDerVal) {
redxeth 0:98e98e01a6ce 566 maxDerVal = DerVal;
redxeth 0:98e98e01a6ce 567 }
redxeth 0:98e98e01a6ce 568 if (DerVal < minDerVal) {
redxeth 0:98e98e01a6ce 569 minDerVal = DerVal;
redxeth 0:98e98e01a6ce 570 }
redxeth 0:98e98e01a6ce 571 aveDerVal = aveDerVal + DerVal; //get sum
redxeth 0:98e98e01a6ce 572 DerivLineScanDataOut[i] = DerVal;
redxeth 0:98e98e01a6ce 573 }
redxeth 0:98e98e01a6ce 574 aveDerVal = (float) aveDerVal / (maxCnt - minCnt);
redxeth 0:98e98e01a6ce 575 }
redxeth 0:98e98e01a6ce 576
redxeth 0:98e98e01a6ce 577 //Not reliable for finding edges!
redxeth 0:98e98e01a6ce 578 void findEdges(float* derivLineScanData)
redxeth 0:98e98e01a6ce 579 {
redxeth 0:98e98e01a6ce 580 // search for edges in deriviative data using a threshold
redxeth 0:98e98e01a6ce 581 // need to store in a hash if that's possible...
redxeth 0:98e98e01a6ce 582 // combine edges that are a pixel apart
redxeth 0:98e98e01a6ce 583
redxeth 0:98e98e01a6ce 584 int i, minCnt = 0, maxCnt = 0;
redxeth 0:98e98e01a6ce 585 int multiNegEdgeCnt = 1, multiNegEdgeSum = 0;
redxeth 0:98e98e01a6ce 586 int multiPosEdgeCnt = 1, multiPosEdgeSum = 0;
redxeth 0:98e98e01a6ce 587
redxeth 0:98e98e01a6ce 588 minCnt = FILTER_ENDS;
redxeth 0:98e98e01a6ce 589 maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
redxeth 0:98e98e01a6ce 590
redxeth 0:98e98e01a6ce 591 numNegEdges = 0;
redxeth 0:98e98e01a6ce 592 numPosEdges = 0;
redxeth 0:98e98e01a6ce 593 for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0
redxeth 0:98e98e01a6ce 594 {
redxeth 0:98e98e01a6ce 595 if (derivLineScanData[i] <= NegDerivThreshold) // NEGATIVE EDGE FOUND!
redxeth 0:98e98e01a6ce 596 {
redxeth 0:98e98e01a6ce 597 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 598 TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
redxeth 0:98e98e01a6ce 599 }
redxeth 0:98e98e01a6ce 600
redxeth 0:98e98e01a6ce 601 if ((numNegEdges > 0) && (NegEdges[numNegEdges - 1] + 1 == i )) // if no multi edges
redxeth 0:98e98e01a6ce 602 { // edge actually across multiple pixels
redxeth 0:98e98e01a6ce 603 multiNegEdgeCnt++;
redxeth 0:98e98e01a6ce 604 multiNegEdgeSum = multiNegEdgeSum + i;
redxeth 0:98e98e01a6ce 605 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 606 TERMINAL_PRINTF("MULTIEDGE FOUND! MultiNegEdgeCnt: %d; MultiNegEdgeSum: %d\r\n", multiNegEdgeCnt, multiNegEdgeSum);
redxeth 0:98e98e01a6ce 607 }
redxeth 0:98e98e01a6ce 608 } else { // not a multi-pixel edge known at this time, store negative edge index value
redxeth 0:98e98e01a6ce 609 numNegEdges++;
redxeth 0:98e98e01a6ce 610 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 611 TERMINAL_PRINTF("NEG EDGE STORED WITH INDEX %d. NUM NEG EDGES = %d\r\n", i, numNegEdges);
redxeth 0:98e98e01a6ce 612 }
redxeth 0:98e98e01a6ce 613 NegEdges[numNegEdges - 1] = (float) i;
redxeth 0:98e98e01a6ce 614 multiNegEdgeSum = i;
redxeth 0:98e98e01a6ce 615 }
redxeth 0:98e98e01a6ce 616
redxeth 0:98e98e01a6ce 617
redxeth 0:98e98e01a6ce 618 } else if (derivLineScanData[i] > PosDerivThreshold) { // POSITIVE EDGE FOUND!
redxeth 0:98e98e01a6ce 619
redxeth 0:98e98e01a6ce 620 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 621 TERMINAL_PRINTF("POS EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
redxeth 0:98e98e01a6ce 622 }
redxeth 0:98e98e01a6ce 623
redxeth 0:98e98e01a6ce 624 if ((numPosEdges > 0) && (PosEdges[numPosEdges - 1] + 1 == i ))
redxeth 0:98e98e01a6ce 625 { // edge actually across multiple pixels
redxeth 0:98e98e01a6ce 626 multiPosEdgeCnt++;
redxeth 0:98e98e01a6ce 627 multiPosEdgeSum = multiPosEdgeSum + i;
redxeth 0:98e98e01a6ce 628 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 629 TERMINAL_PRINTF("MULTIEDGE FOUND! MultiPosEdgeCnt: %d; MultiPosEdgeSum: %d\r\n", multiPosEdgeCnt, multiPosEdgeSum);
redxeth 0:98e98e01a6ce 630 }
redxeth 0:98e98e01a6ce 631 } else { // not a multi-pixel edge known at this time, store Posative edge index value
redxeth 0:98e98e01a6ce 632 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 633 TERMINAL_PRINTF("POS EDGE STORED WITH INDEX %d. NUM POS EDGES = %d\r\n", i, numPosEdges);
redxeth 0:98e98e01a6ce 634 }
redxeth 0:98e98e01a6ce 635 numPosEdges++;
redxeth 0:98e98e01a6ce 636 PosEdges[numPosEdges - 1] = (float) i;
redxeth 0:98e98e01a6ce 637 multiPosEdgeSum = i;
redxeth 0:98e98e01a6ce 638 }
redxeth 0:98e98e01a6ce 639
redxeth 0:98e98e01a6ce 640 } else { // NO EDGE FOUND
redxeth 0:98e98e01a6ce 641 // combine multi-edges if there are any
redxeth 0:98e98e01a6ce 642 if (multiNegEdgeCnt > 1)
redxeth 0:98e98e01a6ce 643 {
redxeth 0:98e98e01a6ce 644 NegEdges[numNegEdges - 1] = (float) multiNegEdgeSum / multiNegEdgeCnt;
redxeth 0:98e98e01a6ce 645 multiNegEdgeCnt = 1; multiNegEdgeSum = 0;
redxeth 0:98e98e01a6ce 646 }
redxeth 0:98e98e01a6ce 647 if (multiPosEdgeCnt > 1)
redxeth 0:98e98e01a6ce 648 {
redxeth 0:98e98e01a6ce 649 PosEdges[numPosEdges - 1] = (float) multiPosEdgeSum / multiPosEdgeCnt;
redxeth 0:98e98e01a6ce 650 multiPosEdgeCnt = 1; multiPosEdgeSum = 0;
redxeth 0:98e98e01a6ce 651 }
redxeth 0:98e98e01a6ce 652
redxeth 0:98e98e01a6ce 653 }
redxeth 0:98e98e01a6ce 654 }
redxeth 0:98e98e01a6ce 655
redxeth 0:98e98e01a6ce 656 }
redxeth 0:98e98e01a6ce 657
redxeth 0:98e98e01a6ce 658
redxeth 0:98e98e01a6ce 659 void findEdges_v2(float* derivLineScanData)
redxeth 0:98e98e01a6ce 660 {
redxeth 0:98e98e01a6ce 661 // search for edges in deriviative data using a threshold
redxeth 0:98e98e01a6ce 662 // need to store in a hash if that's possible...
redxeth 0:98e98e01a6ce 663 // combine edges that are a pixel apart
redxeth 0:98e98e01a6ce 664
redxeth 0:98e98e01a6ce 665 int i;
redxeth 0:98e98e01a6ce 666
redxeth 0:98e98e01a6ce 667 int NegEdgeBufCnt = 0, NegEdgeBufSum = 0; // serves as buffer to store neg edges found next to each other
redxeth 0:98e98e01a6ce 668 int PosEdgeBufCnt = 0, PosEdgeBufSum = 0; // serves as buffer to store pos edges found next to each other
redxeth 0:98e98e01a6ce 669
redxeth 0:98e98e01a6ce 670 int minCnt = FILTER_ENDS;
redxeth 0:98e98e01a6ce 671 int maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
redxeth 0:98e98e01a6ce 672
redxeth 0:98e98e01a6ce 673
redxeth 0:98e98e01a6ce 674
redxeth 0:98e98e01a6ce 675 numNegEdges = 0; // count of neg edges found thus far
redxeth 0:98e98e01a6ce 676 numPosEdges = 0; // count of pos edges found thus far
redxeth 0:98e98e01a6ce 677 for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0
redxeth 0:98e98e01a6ce 678 {
redxeth 0:98e98e01a6ce 679
redxeth 0:98e98e01a6ce 680 if (derivLineScanData[i] <= NegDerivThreshold) // NEGATIVE EDGE FOUND!
redxeth 0:98e98e01a6ce 681 {
redxeth 0:98e98e01a6ce 682
redxeth 0:98e98e01a6ce 683 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 684 TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
redxeth 0:98e98e01a6ce 685 }
redxeth 0:98e98e01a6ce 686
redxeth 0:98e98e01a6ce 687 NegEdgeBufCnt++; // add value to neg edge buffer
redxeth 0:98e98e01a6ce 688 NegEdgeBufSum = NegEdgeBufSum + i;
redxeth 0:98e98e01a6ce 689
redxeth 0:98e98e01a6ce 690 } else if (derivLineScanData[i] > PosDerivThreshold) { // POSITIVE EDGE FOUND!
redxeth 0:98e98e01a6ce 691
redxeth 0:98e98e01a6ce 692 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 693 TERMINAL_PRINTF("POS EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
redxeth 0:98e98e01a6ce 694 }
redxeth 0:98e98e01a6ce 695
redxeth 0:98e98e01a6ce 696 PosEdgeBufCnt++; // add value to pos edge buffer
redxeth 0:98e98e01a6ce 697 PosEdgeBufSum = PosEdgeBufSum + i;
redxeth 0:98e98e01a6ce 698
redxeth 0:98e98e01a6ce 699 } else { // NO EDGE FOUND
redxeth 0:98e98e01a6ce 700
redxeth 0:98e98e01a6ce 701 // POP EDGE BUFFERS IF NON-EMPTY AND STORE TO EDGE "STACK" (i.e. edges found)
redxeth 0:98e98e01a6ce 702
redxeth 0:98e98e01a6ce 703 if (NegEdgeBufCnt > 0) {
redxeth 0:98e98e01a6ce 704 // store edge value
redxeth 0:98e98e01a6ce 705 numNegEdges++;
redxeth 0:98e98e01a6ce 706 NegEdges[numNegEdges - 1] = (float) NegEdgeBufSum / NegEdgeBufCnt;
redxeth 0:98e98e01a6ce 707
redxeth 0:98e98e01a6ce 708 // clear edge buffer
redxeth 0:98e98e01a6ce 709 NegEdgeBufSum = 0; NegEdgeBufCnt = 0;
redxeth 0:98e98e01a6ce 710 }
redxeth 0:98e98e01a6ce 711
redxeth 0:98e98e01a6ce 712 if (PosEdgeBufCnt > 0) {
redxeth 0:98e98e01a6ce 713 // store edge value
redxeth 0:98e98e01a6ce 714 numPosEdges++;
redxeth 0:98e98e01a6ce 715 PosEdges[numPosEdges - 1] = (float) PosEdgeBufSum / PosEdgeBufCnt;
redxeth 0:98e98e01a6ce 716
redxeth 0:98e98e01a6ce 717 // clear edge buffer
redxeth 0:98e98e01a6ce 718 PosEdgeBufSum = 0; PosEdgeBufCnt = 0;
redxeth 0:98e98e01a6ce 719 }
redxeth 0:98e98e01a6ce 720
redxeth 0:98e98e01a6ce 721 }
redxeth 0:98e98e01a6ce 722
redxeth 0:98e98e01a6ce 723 }
redxeth 0:98e98e01a6ce 724
redxeth 0:98e98e01a6ce 725 }
redxeth 0:98e98e01a6ce 726
redxeth 0:98e98e01a6ce 727 void printEdgesFound()
redxeth 0:98e98e01a6ce 728 {
redxeth 0:98e98e01a6ce 729 int i;
redxeth 0:98e98e01a6ce 730
redxeth 0:98e98e01a6ce 731 // Check that neg edges captured ok
redxeth 0:98e98e01a6ce 732 TERMINAL_PRINTF("NEGATIVE EDGES FOUND:,");
redxeth 0:98e98e01a6ce 733 for(i=0;i<=numNegEdges-1;i++)
redxeth 0:98e98e01a6ce 734 {
redxeth 0:98e98e01a6ce 735 TERMINAL_PRINTF("%9.3f",NegEdges[i]);
redxeth 0:98e98e01a6ce 736 if(i==numNegEdges-1) // when last data reached put in line return
redxeth 0:98e98e01a6ce 737 TERMINAL_PRINTF("\r\n");
redxeth 0:98e98e01a6ce 738 else
redxeth 0:98e98e01a6ce 739 TERMINAL_PRINTF(", ");
redxeth 0:98e98e01a6ce 740 }
redxeth 0:98e98e01a6ce 741
redxeth 0:98e98e01a6ce 742
redxeth 0:98e98e01a6ce 743 // Check that pos edges captured ok
redxeth 0:98e98e01a6ce 744 TERMINAL_PRINTF("POSITIVE EDGES FOUND:,");
redxeth 0:98e98e01a6ce 745 for(i=0;i<=numPosEdges-1;i++)
redxeth 0:98e98e01a6ce 746 {
redxeth 0:98e98e01a6ce 747 TERMINAL_PRINTF("%9.3f",PosEdges[i]);
redxeth 0:98e98e01a6ce 748 if(i==numPosEdges-1) // when last data reached put in line return
redxeth 0:98e98e01a6ce 749 TERMINAL_PRINTF("\r\n");
redxeth 0:98e98e01a6ce 750 else
redxeth 0:98e98e01a6ce 751 TERMINAL_PRINTF(", ");
redxeth 0:98e98e01a6ce 752 }
Jmon14 1:7e3c1d7d50b9 753
Jmon14 1:7e3c1d7d50b9 754 TERMINAL_PRINTF("Left Line Found:");
Jmon14 1:7e3c1d7d50b9 755 TERMINAL_PRINTF("%3.1f", CurrentLinePosition1);
Jmon14 1:7e3c1d7d50b9 756 TERMINAL_PRINTF("\r\n");
Jmon14 1:7e3c1d7d50b9 757 TERMINAL_PRINTF("Right Line Found:");
Jmon14 1:7e3c1d7d50b9 758 TERMINAL_PRINTF("%3.1f", CurrentLinePosition2);
Jmon14 1:7e3c1d7d50b9 759 TERMINAL_PRINTF("\r\n");
Jmon14 1:7e3c1d7d50b9 760 TERMINAL_PRINTF("Track Width:");
Jmon14 1:7e3c1d7d50b9 761 TERMINAL_PRINTF("%3.1f", TrackWidth);
Jmon14 1:7e3c1d7d50b9 762 TERMINAL_PRINTF("\r\n");
redxeth 0:98e98e01a6ce 763 }
redxeth 0:98e98e01a6ce 764
redxeth 0:98e98e01a6ce 765 void reviewEdges()
redxeth 0:98e98e01a6ce 766 {
redxeth 0:98e98e01a6ce 767 LastTrackStatus = CurrentTrackStatus;
redxeth 0:98e98e01a6ce 768
Jmon14 1:7e3c1d7d50b9 769 if ((numPosEdges == 1) && (numNegEdges == 1)) // only one negative and positive edge found
redxeth 0:98e98e01a6ce 770 {
Jmon14 1:7e3c1d7d50b9 771 if (((PosEdges[0] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[0] - NegEdges[0]) <= MAX_LINE_WIDTH)) // has proper expected width (1 line found)
redxeth 0:98e98e01a6ce 772 {
Jmon14 1:7e3c1d7d50b9 773 if((LastLinePosError > 0))
Jmon14 1:7e3c1d7d50b9 774 {
Jmon14 1:7e3c1d7d50b9 775 CurrentTrackStatus = LineFound; // report line found!
Jmon14 1:7e3c1d7d50b9 776 UnknownCount = 0; // reset unknown status count
Jmon14 1:7e3c1d7d50b9 777 LastLinePosition = CurrentLinePosition;
Jmon14 1:7e3c1d7d50b9 778 CurrentLinePosition1 = (PosEdges[0]+NegEdges[0]) / 2; // update line position
Jmon14 1:7e3c1d7d50b9 779 CurrentLinePosition2 = CurrentLinePosition1 + TrackWidth; // mayor a 128
Jmon14 1:7e3c1d7d50b9 780 CurrentLinePosition = (CurrentLinePosition1 + CurrentLinePosition2)/2;
Jmon14 1:7e3c1d7d50b9 781 }
Jmon14 1:7e3c1d7d50b9 782
Jmon14 1:7e3c1d7d50b9 783 else
Jmon14 1:7e3c1d7d50b9 784 {
Jmon14 1:7e3c1d7d50b9 785 CurrentTrackStatus = LineFound; // report line found!
Jmon14 1:7e3c1d7d50b9 786 UnknownCount = 0; // reset unknown status count
Jmon14 1:7e3c1d7d50b9 787 LastLinePosition = CurrentLinePosition;
Jmon14 1:7e3c1d7d50b9 788 CurrentLinePosition2 = (PosEdges[0]+NegEdges[0]) / 2; // update line position
Jmon14 1:7e3c1d7d50b9 789 CurrentLinePosition1 = CurrentLinePosition2 - TrackWidth; // menor a 128
Jmon14 1:7e3c1d7d50b9 790 CurrentLinePosition = (CurrentLinePosition1 + CurrentLinePosition2)/2;
Jmon14 1:7e3c1d7d50b9 791 }
redxeth 0:98e98e01a6ce 792 }
Jmon14 1:7e3c1d7d50b9 793 }
Jmon14 1:7e3c1d7d50b9 794
Jmon14 1:7e3c1d7d50b9 795 else if ((numPosEdges == 2) && (numNegEdges == 1)) // 2 posEdges 1 negEdges (posible left line)
Jmon14 1:7e3c1d7d50b9 796 {
Jmon14 1:7e3c1d7d50b9 797 if ((PosEdges[0] <= MAX_LINE_WIDTH) && ((PosEdges[1] - NegEdges[0]) <= MAX_LINE_WIDTH) ) // Maybe left line and right lane
redxeth 0:98e98e01a6ce 798 {
Jmon14 1:7e3c1d7d50b9 799 if(((NegEdges[0]-PosEdges[0]) > (TrackWidth - 15)) && ((NegEdges[0]-PosEdges[0]) < (TrackWidth + 15))) // 2 lines
Jmon14 1:7e3c1d7d50b9 800 {
Jmon14 1:7e3c1d7d50b9 801 CurrentTrackStatus = LineFound; // report line found!
Jmon14 1:7e3c1d7d50b9 802 UnknownCount = 0; // reset unknown status count
Jmon14 1:7e3c1d7d50b9 803 LastLinePosition = CurrentLinePosition;
Jmon14 1:7e3c1d7d50b9 804 CurrentLinePosition1 = PosEdges[0] - ( MAX_LINE_WIDTH / 2); // update line position
Jmon14 1:7e3c1d7d50b9 805 CurrentLinePosition2 = (PosEdges[1] + NegEdges[0])/2;
Jmon14 1:7e3c1d7d50b9 806 TrackWidth = CurrentLinePosition2 - CurrentLinePosition1;
Jmon14 1:7e3c1d7d50b9 807 CurrentLinePosition = (CurrentLinePosition1 + CurrentLinePosition2)/2;
Jmon14 1:7e3c1d7d50b9 808 }
redxeth 0:98e98e01a6ce 809 }
Jmon14 1:7e3c1d7d50b9 810 }
Jmon14 1:7e3c1d7d50b9 811
Jmon14 1:7e3c1d7d50b9 812 else if ((numNegEdges == 2) && (numPosEdges == 1)) // 2 neg edge 1 posEdge found (POSSIBLE LINE right)
Jmon14 1:7e3c1d7d50b9 813 {
Jmon14 1:7e3c1d7d50b9 814 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)
redxeth 0:98e98e01a6ce 815 {
Jmon14 1:7e3c1d7d50b9 816 if(((NegEdges[1]-PosEdges[0]) > (TrackWidth - 15)) && ((NegEdges[1]-PosEdges[0]) < (TrackWidth + 15))) // 2 lines
Jmon14 1:7e3c1d7d50b9 817 {
Jmon14 1:7e3c1d7d50b9 818 CurrentTrackStatus = LineFound; // report line found!
Jmon14 1:7e3c1d7d50b9 819 UnknownCount = 0; // reset unknown status count
Jmon14 1:7e3c1d7d50b9 820 LastLinePosition = CurrentLinePosition;
Jmon14 1:7e3c1d7d50b9 821 CurrentLinePosition1 = (PosEdges[0] + NegEdges[0])/2; // update line position
Jmon14 1:7e3c1d7d50b9 822 CurrentLinePosition2 = (NegEdges[1] + MAX_LINE_WIDTH/2);
Jmon14 1:7e3c1d7d50b9 823 TrackWidth = CurrentLinePosition2 - CurrentLinePosition1;
Jmon14 1:7e3c1d7d50b9 824 CurrentLinePosition = (CurrentLinePosition1 + CurrentLinePosition2)/2;
Jmon14 1:7e3c1d7d50b9 825 }
Jmon14 1:7e3c1d7d50b9 826 }
Jmon14 1:7e3c1d7d50b9 827 }
Jmon14 1:7e3c1d7d50b9 828
Jmon14 1:7e3c1d7d50b9 829 else if ((numPosEdges == 2) && (numNegEdges == 2)) // 2 negative and 2 positive edges found (STARTING/FINISH GATE)
Jmon14 1:7e3c1d7d50b9 830 {
redxeth 0:98e98e01a6ce 831
Jmon14 1:7e3c1d7d50b9 832 if ( ( ( (PosEdges[0] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[0] - NegEdges[0]) <= MAX_LINE_WIDTH)) && // white left 'line'
Jmon14 1:7e3c1d7d50b9 833 (((PosEdges[1] - NegEdges[1]) >= MIN_LINE_WIDTH) && ((PosEdges[1] - NegEdges[1]) <= MAX_LINE_WIDTH)) && // white right 'line'
Jmon14 1:7e3c1d7d50b9 834 ( ( (NegEdges[1] - PosEdges[0]) >= (TrackWidth - 15) ) && ((NegEdges[1] - PosEdges[0]) <= (TrackWidth + 15) ) ) ) // actual track line
Jmon14 1:7e3c1d7d50b9 835 {
Jmon14 1:7e3c1d7d50b9 836
Jmon14 1:7e3c1d7d50b9 837 CurrentTrackStatus = LineFound;
Jmon14 1:7e3c1d7d50b9 838 UnknownCount = 0; // reset unknown status count
Jmon14 1:7e3c1d7d50b9 839 CurrentLinePosition1 = (PosEdges[0] + NegEdges[0])/2; // update line position
Jmon14 1:7e3c1d7d50b9 840 CurrentLinePosition2 = (PosEdges[1] + NegEdges[1])/2;
Jmon14 1:7e3c1d7d50b9 841 TrackWidth = CurrentLinePosition2 - CurrentLinePosition1;
Jmon14 1:7e3c1d7d50b9 842 CurrentLinePosition = (CurrentLinePosition1 + CurrentLinePosition2)/2;
Jmon14 1:7e3c1d7d50b9 843 }
redxeth 0:98e98e01a6ce 844
Jmon14 1:7e3c1d7d50b9 845 }
Jmon14 1:7e3c1d7d50b9 846
Jmon14 1:7e3c1d7d50b9 847 else if ((numPosEdges > 2) && (numNegEdges > 2)) // more than 1 negative edge and positive edge found (but not 2 for both) (STARTING / FINISH GATE)
Jmon14 1:7e3c1d7d50b9 848 {
Jmon14 1:7e3c1d7d50b9 849
Jmon14 1:7e3c1d7d50b9 850 // remove edges that aren't close to center TBD DDHH
Jmon14 1:7e3c1d7d50b9 851
Jmon14 1:7e3c1d7d50b9 852 if (startRaceTicker > STARTGATEDELAY) // only start counting for starting gate until after delay
Jmon14 1:7e3c1d7d50b9 853 {
redxeth 0:98e98e01a6ce 854 StartGateFoundCount++;
redxeth 0:98e98e01a6ce 855 }
redxeth 0:98e98e01a6ce 856
Jmon14 1:7e3c1d7d50b9 857 if (terminalOutput == 3)
Jmon14 1:7e3c1d7d50b9 858 {
redxeth 0:98e98e01a6ce 859 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 860 TERMINAL_PRINTF("********** NOT SURE FOUND ********** \r\n");
redxeth 0:98e98e01a6ce 861 TERMINAL_PRINTF("***************************************** \r\n");
Jmon14 1:7e3c1d7d50b9 862 }
Jmon14 1:7e3c1d7d50b9 863
Jmon14 1:7e3c1d7d50b9 864 CurrentTrackStatus = StartGateFound;
Jmon14 1:7e3c1d7d50b9 865
Jmon14 1:7e3c1d7d50b9 866 }
redxeth 0:98e98e01a6ce 867
Jmon14 1:7e3c1d7d50b9 868 else // no track or starting gate found
Jmon14 1:7e3c1d7d50b9 869
Jmon14 1:7e3c1d7d50b9 870 {
redxeth 0:98e98e01a6ce 871
Jmon14 1:7e3c1d7d50b9 872 if (terminalOutput == 3)
Jmon14 1:7e3c1d7d50b9 873
Jmon14 1:7e3c1d7d50b9 874 {
redxeth 0:98e98e01a6ce 875 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 876 TERMINAL_PRINTF("*** !!!!!!!!!! LINE NOT FOUND !!!!!!! *** \r\n", CurrentLinePosition);
redxeth 0:98e98e01a6ce 877 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 878 }
redxeth 0:98e98e01a6ce 879
redxeth 0:98e98e01a6ce 880 CurrentTrackStatus = Unknown;
redxeth 0:98e98e01a6ce 881 UnknownCount++;
redxeth 0:98e98e01a6ce 882 }
redxeth 0:98e98e01a6ce 883 }
redxeth 0:98e98e01a6ce 884
redxeth 0:98e98e01a6ce 885 void ActOnTrackStatus()
redxeth 0:98e98e01a6ce 886 {
redxeth 0:98e98e01a6ce 887 // Decide what to do next based on current track status
redxeth 0:98e98e01a6ce 888
redxeth 0:98e98e01a6ce 889 if (CurrentTrackStatus == LineFound) { // LINE FOUND!
redxeth 0:98e98e01a6ce 890
redxeth 0:98e98e01a6ce 891 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 892 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 893 TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition);
redxeth 0:98e98e01a6ce 894 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 895 }
redxeth 0:98e98e01a6ce 896
redxeth 0:98e98e01a6ce 897 // Update steering position
redxeth 0:98e98e01a6ce 898 SteeringControl();
redxeth 0:98e98e01a6ce 899
redxeth 0:98e98e01a6ce 900 // Apply to servo
redxeth 0:98e98e01a6ce 901 Steer();
redxeth 0:98e98e01a6ce 902
redxeth 0:98e98e01a6ce 903 } else if (CurrentTrackStatus == StartGateFound) { // STARTING GATE FOUND
redxeth 0:98e98e01a6ce 904
redxeth 0:98e98e01a6ce 905 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 906 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 907 TERMINAL_PRINTF("********** STARTING GATE FOUND ********** \r\n");
redxeth 0:98e98e01a6ce 908 TERMINAL_PRINTF("********** count = %d ********** \r\n", StartGateFoundCount);
redxeth 0:98e98e01a6ce 909 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 910 }
redxeth 0:98e98e01a6ce 911
redxeth 0:98e98e01a6ce 912 // END RACE!
redxeth 0:98e98e01a6ce 913 if (startGateStop) {
redxeth 0:98e98e01a6ce 914 if (StartGateFoundCount > STARTGATEFOUNDMAX)
redxeth 0:98e98e01a6ce 915 {
redxeth 0:98e98e01a6ce 916 go = false; // STOP!!
redxeth 0:98e98e01a6ce 917 }
redxeth 0:98e98e01a6ce 918 }
redxeth 0:98e98e01a6ce 919
redxeth 0:98e98e01a6ce 920 }
redxeth 0:98e98e01a6ce 921 }
redxeth 0:98e98e01a6ce 922
redxeth 0:98e98e01a6ce 923 void SteeringControl()
redxeth 0:98e98e01a6ce 924 {
redxeth 0:98e98e01a6ce 925
redxeth 0:98e98e01a6ce 926 float targetPosition = (float)( (NUM_LINE_SCAN / 2) - 0.5); // target to achieve for line position
redxeth 0:98e98e01a6ce 927
redxeth 0:98e98e01a6ce 928 float KP; // proportional control factor
redxeth 0:98e98e01a6ce 929 float KI; // integral control factor
redxeth 0:98e98e01a6ce 930 float KD; // derivative control factor
redxeth 0:98e98e01a6ce 931
redxeth 0:98e98e01a6ce 932 float Pout, Iout, Dout; // PID terms
redxeth 0:98e98e01a6ce 933
redxeth 0:98e98e01a6ce 934 // Calculate error
redxeth 0:98e98e01a6ce 935 // make error to the right positive
redxeth 0:98e98e01a6ce 936 // i.e. if LINE to the right-- then CurrentLinePosError > 0
redxeth 0:98e98e01a6ce 937 // if LINE to the left -- then CurrentLinePosError < 0
redxeth 0:98e98e01a6ce 938 CurrentLinePosError = CurrentLinePosition - targetPosition;
redxeth 0:98e98e01a6ce 939
redxeth 0:98e98e01a6ce 940 // Get absolute error
redxeth 0:98e98e01a6ce 941 if (CurrentLinePosError >= 0)
redxeth 0:98e98e01a6ce 942 AbsError = CurrentLinePosError;
redxeth 0:98e98e01a6ce 943 else
redxeth 0:98e98e01a6ce 944 AbsError = -1 * CurrentLinePosError;
redxeth 0:98e98e01a6ce 945
redxeth 0:98e98e01a6ce 946 // CHOOSE SET OF PID CONTROL PARAMETERS
redxeth 0:98e98e01a6ce 947 switch (CONTROL_METHOD) {
redxeth 0:98e98e01a6ce 948 case 0:
redxeth 0:98e98e01a6ce 949 // Pure proportional control based on range of steering values vs. range of error values
redxeth 0:98e98e01a6ce 950 KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT ) / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH );
redxeth 0:98e98e01a6ce 951 KD = 0;
redxeth 0:98e98e01a6ce 952 KI = 0;
redxeth 0:98e98e01a6ce 953 break;
redxeth 0:98e98e01a6ce 954 case 1:
redxeth 0:98e98e01a6ce 955 // Proportional control with 50% bit more oomph --- a bit more aggressive around the bends
redxeth 0:98e98e01a6ce 956 KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT ) / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH );
redxeth 0:98e98e01a6ce 957 KP = KP * 1.5;
redxeth 0:98e98e01a6ce 958 KD = 0;
redxeth 0:98e98e01a6ce 959 KI = 0;
redxeth 0:98e98e01a6ce 960 break;
redxeth 0:98e98e01a6ce 961 case 2: // MANUAL TUNING CASE 1 (use pot to help determine tuning parameters)
redxeth 0:98e98e01a6ce 962 KP = TUNE_KP;
redxeth 0:98e98e01a6ce 963 KI = TUNE_KI;
redxeth 0:98e98e01a6ce 964 KD = TUNE_KD;
Jmon14 1:7e3c1d7d50b9 965 break;
redxeth 0:98e98e01a6ce 966 case 3:
redxeth 0:98e98e01a6ce 967 if (AbsError < ABS_ERROR_THRESH) {
redxeth 0:98e98e01a6ce 968 KP = 0.003; // when relatively straight, keep KP gain low
redxeth 0:98e98e01a6ce 969 } else {
redxeth 0:98e98e01a6ce 970 KP = 0.010; // when curve begins or off track, increase KP gain
redxeth 0:98e98e01a6ce 971 }
redxeth 0:98e98e01a6ce 972 KI = 0;
redxeth 0:98e98e01a6ce 973 KD = 0;
Jmon14 1:7e3c1d7d50b9 974 break;
redxeth 0:98e98e01a6ce 975
redxeth 0:98e98e01a6ce 976 default:
redxeth 0:98e98e01a6ce 977 break;
redxeth 0:98e98e01a6ce 978 }
redxeth 0:98e98e01a6ce 979
redxeth 0:98e98e01a6ce 980 /* Pseudocode
redxeth 0:98e98e01a6ce 981 previous_error = 0
redxeth 0:98e98e01a6ce 982 integral = 0
redxeth 0:98e98e01a6ce 983 start:
redxeth 0:98e98e01a6ce 984 error = setpoint - measured_value
redxeth 0:98e98e01a6ce 985 integral = integral + error*dt
redxeth 0:98e98e01a6ce 986 derivative = (error - previous_error)/dt
redxeth 0:98e98e01a6ce 987 output = Kp*error + Ki*integral + Kd*derivative
redxeth 0:98e98e01a6ce 988 previous_error = error
redxeth 0:98e98e01a6ce 989 wait(dt)
redxeth 0:98e98e01a6ce 990 goto start
redxeth 0:98e98e01a6ce 991 */
redxeth 0:98e98e01a6ce 992
redxeth 0:98e98e01a6ce 993
redxeth 0:98e98e01a6ce 994 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 995 TERMINAL_PRINTF("KP = %6.4f\r\n", KP);
redxeth 0:98e98e01a6ce 996 TERMINAL_PRINTF("TARGET %6.3f\r\n", targetPosition);
redxeth 0:98e98e01a6ce 997 }
redxeth 0:98e98e01a6ce 998
redxeth 0:98e98e01a6ce 999
redxeth 0:98e98e01a6ce 1000
redxeth 0:98e98e01a6ce 1001 // Update integral of error
redxeth 0:98e98e01a6ce 1002 // i.e. if LINE stays to the right, then SumLinePosError increases
redxeth 0:98e98e01a6ce 1003 // i.e. if LINE stays to the left, then SumLinePosError decreases
redxeth 0:98e98e01a6ce 1004 SumLinePosError = SumLinePosError + ( CurrentLinePosError * DT );
redxeth 0:98e98e01a6ce 1005
redxeth 0:98e98e01a6ce 1006 DerivError = (CurrentLinePosError - LastLinePosError) / DT;
redxeth 0:98e98e01a6ce 1007
redxeth 0:98e98e01a6ce 1008 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 1009 TERMINAL_PRINTF("CURRENT LINE POSITION %9.3f\r\n", CurrentLinePosition);
redxeth 0:98e98e01a6ce 1010 TERMINAL_PRINTF("CURRENT LINE POSITION ERROR %9.3f\r\n", CurrentLinePosError);
redxeth 0:98e98e01a6ce 1011 }
redxeth 0:98e98e01a6ce 1012
redxeth 0:98e98e01a6ce 1013 // SECOND- calculate new servo position
redxeth 0:98e98e01a6ce 1014
redxeth 0:98e98e01a6ce 1015 // proportional control term
redxeth 0:98e98e01a6ce 1016 Pout = KP * CurrentLinePosError;
redxeth 0:98e98e01a6ce 1017
redxeth 0:98e98e01a6ce 1018 // integral control term
redxeth 0:98e98e01a6ce 1019 Iout = KI * SumLinePosError;
redxeth 0:98e98e01a6ce 1020
redxeth 0:98e98e01a6ce 1021 // Derivative control term
redxeth 0:98e98e01a6ce 1022 Dout = KD * DerivError;
redxeth 0:98e98e01a6ce 1023
redxeth 0:98e98e01a6ce 1024 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 1025 TERMINAL_PRINTF("KP = %6.4f\r\n", KP);
redxeth 0:98e98e01a6ce 1026 TERMINAL_PRINTF("KI = %6.4f\r\n", KI);
redxeth 0:98e98e01a6ce 1027 TERMINAL_PRINTF("KD = %6.4f\r\n", KD);
redxeth 0:98e98e01a6ce 1028 TERMINAL_PRINTF("Pout = %6.4f\r\n", Pout);
redxeth 0:98e98e01a6ce 1029 TERMINAL_PRINTF("Iout = %6.4f\r\n", Iout);
redxeth 0:98e98e01a6ce 1030 TERMINAL_PRINTF("Dout = %6.4f\r\n", Dout);
redxeth 0:98e98e01a6ce 1031 }
redxeth 0:98e98e01a6ce 1032
redxeth 0:98e98e01a6ce 1033 // Finally add offset to steering to account for non-centered servo mounting
redxeth 0:98e98e01a6ce 1034 // CurrentSteerSetting = Pout + Iout + Dout + ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 );
Jmon14 1:7e3c1d7d50b9 1035 CurrentSteerSetting = ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 ) - Pout ;
redxeth 0:98e98e01a6ce 1036
redxeth 0:98e98e01a6ce 1037 // store for next cycle deriv calculation
redxeth 0:98e98e01a6ce 1038 LastLinePosError = CurrentLinePosError;
redxeth 0:98e98e01a6ce 1039
redxeth 0:98e98e01a6ce 1040 // for tuning control algo only
redxeth 0:98e98e01a6ce 1041 if (1 == 0) {
redxeth 0:98e98e01a6ce 1042 TERMINAL_PRINTF("*** ******************************** \r\n");
redxeth 0:98e98e01a6ce 1043 TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition);
redxeth 0:98e98e01a6ce 1044 TERMINAL_PRINTF("*** ERROR %9.3f *** \r\n", CurrentLinePosError);
redxeth 0:98e98e01a6ce 1045 TERMINAL_PRINTF("*** INTEGRAL ERROR %9.3f *** \r\n", SumLinePosError);
redxeth 0:98e98e01a6ce 1046 TERMINAL_PRINTF("*** DERIVATIVE ERROR %9.3f *** \r\n", DerivError);
redxeth 0:98e98e01a6ce 1047 TERMINAL_PRINTF("*** P STEER SETTING %9.3f *** \r\n", CurrentSteerSetting);
redxeth 0:98e98e01a6ce 1048 TERMINAL_PRINTF("*** PI STEER SETTING %9.3f *** \r\n", (CurrentSteerSetting + Iout));
redxeth 0:98e98e01a6ce 1049 TERMINAL_PRINTF("*** ******************************** \r\n");
redxeth 0:98e98e01a6ce 1050 wait_ms(1000);
redxeth 0:98e98e01a6ce 1051 }
redxeth 0:98e98e01a6ce 1052
redxeth 0:98e98e01a6ce 1053 }
redxeth 0:98e98e01a6ce 1054
redxeth 0:98e98e01a6ce 1055 void Steer()
redxeth 0:98e98e01a6ce 1056 {
redxeth 0:98e98e01a6ce 1057
redxeth 0:98e98e01a6ce 1058 // make sure doesn't go beyond steering limits
redxeth 0:98e98e01a6ce 1059 if (CurrentSteerSetting > MAX_STEER_RIGHT)
redxeth 0:98e98e01a6ce 1060 {
redxeth 0:98e98e01a6ce 1061 CurrentSteerSetting = MAX_STEER_RIGHT;
redxeth 0:98e98e01a6ce 1062 } else if (CurrentSteerSetting < MAX_STEER_LEFT)
redxeth 0:98e98e01a6ce 1063 {
redxeth 0:98e98e01a6ce 1064 CurrentSteerSetting = MAX_STEER_LEFT;
redxeth 0:98e98e01a6ce 1065 }
redxeth 0:98e98e01a6ce 1066
redxeth 0:98e98e01a6ce 1067 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 1068 TERMINAL_PRINTF("APPLYING SERVO SETTING %5.3f\r\n", CurrentSteerSetting);
redxeth 0:98e98e01a6ce 1069 }
redxeth 0:98e98e01a6ce 1070 TFC_SetServo(0,CurrentSteerSetting);
redxeth 0:98e98e01a6ce 1071
redxeth 0:98e98e01a6ce 1072 }
redxeth 0:98e98e01a6ce 1073
redxeth 0:98e98e01a6ce 1074 void SpeedControl()
redxeth 0:98e98e01a6ce 1075 {
redxeth 0:98e98e01a6ce 1076
redxeth 0:98e98e01a6ce 1077 // Get max speed setting from reading pot0
redxeth 0:98e98e01a6ce 1078 // then adjust
redxeth 0:98e98e01a6ce 1079
redxeth 0:98e98e01a6ce 1080 float ErrLimit;
redxeth 0:98e98e01a6ce 1081 float LeftDriveRatio, RightDriveRatio;
redxeth 0:98e98e01a6ce 1082
redxeth 0:98e98e01a6ce 1083 // set maximum speed allowed
redxeth 0:98e98e01a6ce 1084 switch (1)
redxeth 0:98e98e01a6ce 1085 {
redxeth 0:98e98e01a6ce 1086 case 0:
redxeth 0:98e98e01a6ce 1087 // read value off pot0
redxeth 0:98e98e01a6ce 1088 MaxSpeed = TFC_ReadPot(0);
redxeth 0:98e98e01a6ce 1089 break;
redxeth 0:98e98e01a6ce 1090 case 1:
redxeth 0:98e98e01a6ce 1091 if (doRisky)
redxeth 0:98e98e01a6ce 1092 MaxSpeed = TUNE_SPEED + 0.1;
redxeth 0:98e98e01a6ce 1093 else
redxeth 0:98e98e01a6ce 1094 MaxSpeed = TUNE_SPEED;
redxeth 0:98e98e01a6ce 1095 break;
redxeth 0:98e98e01a6ce 1096 case 2:
redxeth 0:98e98e01a6ce 1097 MaxSpeed = SUB_LIGHT_SPEED;
redxeth 0:98e98e01a6ce 1098 break;
redxeth 0:98e98e01a6ce 1099 case 3:
redxeth 0:98e98e01a6ce 1100 MaxSpeed = LIGHT_SPEED;
redxeth 0:98e98e01a6ce 1101 break;
redxeth 0:98e98e01a6ce 1102 case 4:
redxeth 0:98e98e01a6ce 1103 MaxSpeed = RIDICULOUS_SPEED;
redxeth 0:98e98e01a6ce 1104 break;
redxeth 0:98e98e01a6ce 1105 case 5:
redxeth 0:98e98e01a6ce 1106 MaxSpeed = LUDICROUS_SPEED;
redxeth 0:98e98e01a6ce 1107 break;
redxeth 0:98e98e01a6ce 1108 default:
redxeth 0:98e98e01a6ce 1109 break;
redxeth 0:98e98e01a6ce 1110 }
redxeth 0:98e98e01a6ce 1111
redxeth 0:98e98e01a6ce 1112 switch (SPEED_ADJUST)
redxeth 0:98e98e01a6ce 1113 {
redxeth 0:98e98e01a6ce 1114 case 0:
redxeth 0:98e98e01a6ce 1115 // SPEED ADJUST METHOD 0
redxeth 0:98e98e01a6ce 1116 // no speed adjust
redxeth 0:98e98e01a6ce 1117 LeftDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1118 RightDriveRatio = LeftDriveRatio;
redxeth 0:98e98e01a6ce 1119 case 1:
redxeth 0:98e98e01a6ce 1120 // SPEED ADJUST METHOD 1
redxeth 0:98e98e01a6ce 1121 // High speed when error is low, low speed when error is high
redxeth 0:98e98e01a6ce 1122 // lower speed when more than third outside of center
redxeth 0:98e98e01a6ce 1123 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.33;
redxeth 0:98e98e01a6ce 1124 if (AbsError > ErrLimit) {
redxeth 0:98e98e01a6ce 1125 LeftDriveRatio = MIN_POWER;
redxeth 0:98e98e01a6ce 1126 } else {
redxeth 0:98e98e01a6ce 1127 LeftDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1128 }
redxeth 0:98e98e01a6ce 1129 RightDriveRatio = LeftDriveRatio;
redxeth 0:98e98e01a6ce 1130 break;
redxeth 0:98e98e01a6ce 1131 case 2:
redxeth 0:98e98e01a6ce 1132 // SPEED ADJUST METHOD 2
redxeth 0:98e98e01a6ce 1133 // Have max/min speed adjust proportional to absolute value of line error
redxeth 0:98e98e01a6ce 1134 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO;
redxeth 0:98e98e01a6ce 1135 LeftDriveRatio = MAX_POWER - ((MAX_POWER - MIN_POWER) * (AbsError / ErrLimit));
redxeth 0:98e98e01a6ce 1136 RightDriveRatio = LeftDriveRatio;
redxeth 0:98e98e01a6ce 1137 break;
redxeth 0:98e98e01a6ce 1138 case 3:
redxeth 0:98e98e01a6ce 1139 // SPEED ADJUST METHOD 3
redxeth 0:98e98e01a6ce 1140 // have wheel relative speed proportional to absolute value of line error
redxeth 0:98e98e01a6ce 1141 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO;
redxeth 0:98e98e01a6ce 1142 if (CurrentLinePosError > 0) { // heading right
redxeth 0:98e98e01a6ce 1143 LeftDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1144 RightDriveRatio = (MIN_POWER - MAX_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ) + MAX_POWER;
redxeth 0:98e98e01a6ce 1145 } else if (CurrentLinePosError < 0) { // heading left
redxeth 0:98e98e01a6ce 1146 RightDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1147 LeftDriveRatio = (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ) + MAX_POWER;
redxeth 0:98e98e01a6ce 1148 } else {
redxeth 0:98e98e01a6ce 1149 LeftDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1150 RightDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1151 }
redxeth 0:98e98e01a6ce 1152 break;
redxeth 0:98e98e01a6ce 1153 case 4:
redxeth 0:98e98e01a6ce 1154 // SPEED ADJUST METHOD 4
redxeth 0:98e98e01a6ce 1155 // have wheel relative speed proportional to absolute value of line error
redxeth 0:98e98e01a6ce 1156 // only when above a certain error
redxeth 0:98e98e01a6ce 1157 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.1;
redxeth 0:98e98e01a6ce 1158 if (CurrentLinePosError > ErrLimit) { // heading right
redxeth 0:98e98e01a6ce 1159 LeftDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) );
redxeth 0:98e98e01a6ce 1160 RightDriveRatio = MIN_POWER;
redxeth 0:98e98e01a6ce 1161 } else if (CurrentLinePosError < (-1 * ErrLimit)) { // heading left
redxeth 0:98e98e01a6ce 1162 RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) );
redxeth 0:98e98e01a6ce 1163 LeftDriveRatio = MIN_POWER;
redxeth 0:98e98e01a6ce 1164 } else {
redxeth 0:98e98e01a6ce 1165 LeftDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1166 RightDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1167 }
redxeth 0:98e98e01a6ce 1168 break;
redxeth 0:98e98e01a6ce 1169 case 5:
redxeth 0:98e98e01a6ce 1170 // SPEED ADJUST METHOD 5
redxeth 0:98e98e01a6ce 1171 // High speed when error is low, low speed when error is high
redxeth 0:98e98e01a6ce 1172 // lower speed when more than third outside of center
redxeth 0:98e98e01a6ce 1173 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.2;
redxeth 0:98e98e01a6ce 1174 if (AbsError > ErrLimit) {
redxeth 0:98e98e01a6ce 1175 LeftDriveRatio = MIN_POWER;
redxeth 0:98e98e01a6ce 1176 } else {
redxeth 0:98e98e01a6ce 1177 LeftDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1178 }
redxeth 0:98e98e01a6ce 1179 RightDriveRatio = LeftDriveRatio;
redxeth 0:98e98e01a6ce 1180 break;
redxeth 0:98e98e01a6ce 1181 case 6:
redxeth 0:98e98e01a6ce 1182 // SPEED ADJUST METHOD 6
redxeth 0:98e98e01a6ce 1183 // High speed when error is low, low speed when error is high
redxeth 0:98e98e01a6ce 1184 // lower speed when more than third outside of center
redxeth 0:98e98e01a6ce 1185 if (AbsError > ABS_ERROR_THRESH) {
redxeth 0:98e98e01a6ce 1186 LeftDriveRatio = MIN_POWER;
redxeth 0:98e98e01a6ce 1187 } else {
redxeth 0:98e98e01a6ce 1188 LeftDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1189 }
redxeth 0:98e98e01a6ce 1190 RightDriveRatio = LeftDriveRatio;
redxeth 0:98e98e01a6ce 1191 break;
redxeth 0:98e98e01a6ce 1192 default:
redxeth 0:98e98e01a6ce 1193 break;
redxeth 0:98e98e01a6ce 1194
redxeth 0:98e98e01a6ce 1195 }
redxeth 0:98e98e01a6ce 1196 // TBD-- add speed adjust based on Xaccel sensor!
redxeth 0:98e98e01a6ce 1197
redxeth 0:98e98e01a6ce 1198
redxeth 0:98e98e01a6ce 1199 // currently no control mechanism as don't have speed sensor
redxeth 0:98e98e01a6ce 1200 CurrentLeftDriveSetting = (float) (LeftDriveRatio / 100) * MaxSpeed;
redxeth 0:98e98e01a6ce 1201 CurrentRightDriveSetting = (float) (RightDriveRatio / 100) * MaxSpeed;
redxeth 0:98e98e01a6ce 1202
redxeth 0:98e98e01a6ce 1203
redxeth 0:98e98e01a6ce 1204 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 1205 TERMINAL_PRINTF("Abs Error: %4.2f\r\n", AbsError);
redxeth 0:98e98e01a6ce 1206 TERMINAL_PRINTF("Error Limit: %4.2f\r\n", ErrLimit);
Jmon14 1:7e3c1d7d50b9 1207 TERMINAL_PRINTF("MAX SPEED = %5.2f\r\n", MaxSpeed);
redxeth 0:98e98e01a6ce 1208 TERMINAL_PRINTF("Current Left Drive Setting: %5.2f\r\n", CurrentLeftDriveSetting);
redxeth 0:98e98e01a6ce 1209 TERMINAL_PRINTF("Current Right Drive Setting: %5.2f\r\n", CurrentRightDriveSetting);
redxeth 0:98e98e01a6ce 1210 }
redxeth 0:98e98e01a6ce 1211 if (1 == 0) {
redxeth 0:98e98e01a6ce 1212 TERMINAL_PRINTF("Current Left Drive Setting: %5.2f\r\n", CurrentLeftDriveSetting);
redxeth 0:98e98e01a6ce 1213 TERMINAL_PRINTF("Current Right Drive Setting: %5.2f\r\n", CurrentRightDriveSetting);
redxeth 0:98e98e01a6ce 1214 }
redxeth 0:98e98e01a6ce 1215
redxeth 0:98e98e01a6ce 1216 }
redxeth 0:98e98e01a6ce 1217
redxeth 0:98e98e01a6ce 1218 void Drive()
redxeth 0:98e98e01a6ce 1219 {
redxeth 0:98e98e01a6ce 1220
redxeth 0:98e98e01a6ce 1221 // START!
redxeth 0:98e98e01a6ce 1222 // if not going, go when button A is pressed
redxeth 0:98e98e01a6ce 1223 if (!go) {
redxeth 0:98e98e01a6ce 1224 if(TFC_PUSH_BUTTON_0_PRESSED) {
redxeth 0:98e98e01a6ce 1225 go = true;
redxeth 0:98e98e01a6ce 1226 UnknownCount = 0;
redxeth 0:98e98e01a6ce 1227 StartGateFoundCount = 0;
redxeth 0:98e98e01a6ce 1228 startRaceTicker = TFC_Ticker[0]; // keep track of start of race
redxeth 0:98e98e01a6ce 1229 logDataIndex = 0; // reset log data index
redxeth 0:98e98e01a6ce 1230 }
redxeth 0:98e98e01a6ce 1231 }
redxeth 0:98e98e01a6ce 1232
redxeth 0:98e98e01a6ce 1233 // STOP!
redxeth 0:98e98e01a6ce 1234 // if going, stop when button A is pressed
redxeth 0:98e98e01a6ce 1235 if (go) {
redxeth 0:98e98e01a6ce 1236 if(TFC_PUSH_BUTTON_1_PRESSED) {
redxeth 0:98e98e01a6ce 1237 go = false;
redxeth 0:98e98e01a6ce 1238 StartGateFoundCount = 0;
redxeth 0:98e98e01a6ce 1239 }
redxeth 0:98e98e01a6ce 1240 }
redxeth 0:98e98e01a6ce 1241
redxeth 0:98e98e01a6ce 1242 // EMERGENCY STOP!
redxeth 0:98e98e01a6ce 1243 // 'kill switch' to prevent crashes off-track
redxeth 0:98e98e01a6ce 1244 if (killSwitch) {
redxeth 0:98e98e01a6ce 1245 if (UnknownCount > UNKNOWN_COUNT_MAX) { // if track not found after certain time
redxeth 0:98e98e01a6ce 1246 go = false; // kill engine
redxeth 0:98e98e01a6ce 1247 StartGateFoundCount = 0;
redxeth 0:98e98e01a6ce 1248 }
redxeth 0:98e98e01a6ce 1249 }
redxeth 0:98e98e01a6ce 1250
redxeth 0:98e98e01a6ce 1251 // ****************
redxeth 0:98e98e01a6ce 1252
redxeth 0:98e98e01a6ce 1253 if (!go) { // stop!
redxeth 0:98e98e01a6ce 1254 TFC_SetMotorPWM(0,0); //Make sure motors are off
redxeth 0:98e98e01a6ce 1255 TFC_HBRIDGE_DISABLE;
redxeth 0:98e98e01a6ce 1256 }
redxeth 0:98e98e01a6ce 1257
redxeth 0:98e98e01a6ce 1258 if (go) { // go!
redxeth 0:98e98e01a6ce 1259 TFC_HBRIDGE_ENABLE;
redxeth 0:98e98e01a6ce 1260 // motor A = right, motor B = left based on way it is mounted
redxeth 0:98e98e01a6ce 1261 TFC_SetMotorPWM(CurrentRightDriveSetting,CurrentLeftDriveSetting);
redxeth 0:98e98e01a6ce 1262 }
redxeth 0:98e98e01a6ce 1263 }
redxeth 0:98e98e01a6ce 1264
redxeth 0:98e98e01a6ce 1265
redxeth 0:98e98e01a6ce 1266 void adjustLights()
redxeth 0:98e98e01a6ce 1267 {
redxeth 0:98e98e01a6ce 1268
redxeth 0:98e98e01a6ce 1269 // LIGHT ADJUST METHOD 1
redxeth 0:98e98e01a6ce 1270 // threshold is 1/5 of light intensity 'range'
redxeth 0:98e98e01a6ce 1271 if (1 == 0) {
redxeth 0:98e98e01a6ce 1272 DerivThreshold = (float) (MaxLightIntensity - MinLightIntensity) / 5;
redxeth 0:98e98e01a6ce 1273 NegDerivThreshold = (float) -1 * (DerivThreshold);
redxeth 0:98e98e01a6ce 1274 PosDerivThreshold = (float) (DerivThreshold);
redxeth 0:98e98e01a6ce 1275 } else {
redxeth 0:98e98e01a6ce 1276 // LIGHT ADJUST METHOD 2 -- SEEMS TO WORK MUCH BETTER
redxeth 0:98e98e01a6ce 1277 // pos edge threshold is half range of max deriv above aver derive
redxeth 0:98e98e01a6ce 1278 // neg edge threshold is half range of min deriv above aver derive
redxeth 0:98e98e01a6ce 1279
redxeth 0:98e98e01a6ce 1280 NegDerivThreshold = (float) (minDerVal - aveDerVal) * DER_RATIO;
redxeth 0:98e98e01a6ce 1281 PosDerivThreshold = (float) (maxDerVal - aveDerVal) * DER_RATIO;
redxeth 0:98e98e01a6ce 1282
redxeth 0:98e98e01a6ce 1283 }
redxeth 0:98e98e01a6ce 1284
redxeth 0:98e98e01a6ce 1285 printAdjustLightsData();
redxeth 0:98e98e01a6ce 1286
redxeth 0:98e98e01a6ce 1287 }
redxeth 0:98e98e01a6ce 1288
redxeth 0:98e98e01a6ce 1289 void printAdjustLightsData()
redxeth 0:98e98e01a6ce 1290 {
redxeth 0:98e98e01a6ce 1291 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 1292 TERMINAL_PRINTF("Max Light Intensity: %4d\r\n", MaxLightIntensity);
redxeth 0:98e98e01a6ce 1293 TERMINAL_PRINTF("Min Light Intensity: %4d\r\n", MinLightIntensity);
Jmon14 1:7e3c1d7d50b9 1294 TERMINAL_PRINTF("Deriv Threshold: %9.3f\r\n", NegDerivThreshold);
Jmon14 1:7e3c1d7d50b9 1295 TERMINAL_PRINTF("Deriv Threshold: %9.3f\r\n", PosDerivThreshold);
redxeth 0:98e98e01a6ce 1296 }
redxeth 0:98e98e01a6ce 1297
redxeth 0:98e98e01a6ce 1298 }
redxeth 0:98e98e01a6ce 1299
redxeth 0:98e98e01a6ce 1300 void feedbackLights()
redxeth 0:98e98e01a6ce 1301 {
redxeth 0:98e98e01a6ce 1302 switch (CurrentTrackStatus)
redxeth 0:98e98e01a6ce 1303 {
redxeth 0:98e98e01a6ce 1304 case LineFound:
redxeth 0:98e98e01a6ce 1305 TFC_BAT_LED0_OFF;
redxeth 0:98e98e01a6ce 1306 TFC_BAT_LED1_ON;
redxeth 0:98e98e01a6ce 1307 TFC_BAT_LED2_ON;
redxeth 0:98e98e01a6ce 1308 TFC_BAT_LED3_OFF;
redxeth 0:98e98e01a6ce 1309 break;
redxeth 0:98e98e01a6ce 1310 case StartGateFound:
redxeth 0:98e98e01a6ce 1311 TFC_BAT_LED0_ON;
redxeth 0:98e98e01a6ce 1312 TFC_BAT_LED1_OFF;
redxeth 0:98e98e01a6ce 1313 TFC_BAT_LED2_OFF;
redxeth 0:98e98e01a6ce 1314 TFC_BAT_LED3_ON;
redxeth 0:98e98e01a6ce 1315 break;
redxeth 0:98e98e01a6ce 1316 default:
redxeth 0:98e98e01a6ce 1317 TFC_BAT_LED0_OFF;
redxeth 0:98e98e01a6ce 1318 TFC_BAT_LED1_OFF;
redxeth 0:98e98e01a6ce 1319 TFC_BAT_LED2_OFF;
redxeth 0:98e98e01a6ce 1320 TFC_BAT_LED3_OFF;
redxeth 0:98e98e01a6ce 1321 }
redxeth 0:98e98e01a6ce 1322
redxeth 0:98e98e01a6ce 1323 }
redxeth 0:98e98e01a6ce 1324