RACE CAR
Fork of TFC-RACING-DEMO by
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?
User | Revision | Line number | New 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 |