Daniel Hadad
This is an example program that actually allows the car to race using the FRDM-TFC library!
Fork of
TFC-RACING-DEMO
by 
Daniel Hadad
Overview
This is an example program that uses the FRDM-TFC library to actually permit a car using the FDRM-TFC shield + FRDM-KL25Z to race around a center line track.
Exercises designed for Mentoring Matters Car Summer Camp for Summer 2014 at Freescale, Inc. in Austin, Texas
Car MODES
5 MODES OR EXERCISES THAT WILL ALIGN WITH DIP SWITCH SETTINGS IN BINARY FASHION e.g. Mode 1 = 001 = switch 1 is on, switch 2 is off, switch 3 is off
0 = 000 = Garage Mode, button light test to see if car alive!!
- PUSHBUTTON A - Light LEDs 0 and 1
- PUSHBUTTON B - Light LEDs 2 and 3
- (switch 4 does nothing)
1 = 001 = Garage Mode, forward/reverse adjust, no auto steer, terminal output
- POT 1 - Controls speed of right wheel: CCW = reverse; CW = forward
- POT 2 - Controls speed of left wheel: CCW = reverse; CW = forward
- NOTE In this mode the high pitched whine heard is of the H-bridge being active. To disable whine, briefly put into demo mode 1 above.
- (switch 4 does nothing)
2 = 010 = Garage Mode, steering adjust, no auto steer, terminal output
- POT 1 - Controls Servo: CCW = left; CW = right
- (switch 4 does nothing)
3 = 011 = Garage Mode, Camera test, some auto steer, terminal output
- switch 4 : OFF = normal dec data, ON = o-scope mode
4 = 100 = Track Mode, Auto Steer, safe settings
- LIGHT SPEED = 0.4 default
- switch 4 = terminal output on/off
- Pot0 = controls motor speed
- Pot1 = controls value to multiply with Kp for proportional control
5 = 101 = Track Mode, Auto Steer, fast settings
- LUDICROUS SPEED = 0.6 default
- switch 4 = terminal output on/off
- Pot0 = controls motor speed
- Pot1 = controls value to multiply with Kp for proportional control
6 = 110 = future upgrades
7 = 111 = future upgrades
Car Hookup
Motors
- Code written assuming left motor hooked to B1(red)/B2(black) and right motor hooked to A1(red)/A2(black).
Battery
- Be sure to hook positive (red) to 'Vbat' and negative (black) to 'Gnd'
Steering Servo
- Servo must be hooked up with black wire innermost (away from LEDs).
- Also be sure to mount servo on chassis with wire coming out the right side of the car.
Camera
- Camera must be hooked up with black wire towards LEDs on the shield board.
- Be sure to mount camera on tower with connector down towards the bottom.
Potentiometers (Pots)
- Pots by default should have arrows pointing toward battery/motor hookup (for demo mods default).
Car Calibration
Serial Terminal
- Download your favorite Terminal application (I use TeraTerm. Set it for 115200 baud, 8bit, none, 1bit.
- But first you have to be sure that Windows mbed serial driver has been installed: Windows serial config.
Camera
- Be sure to focus the camera properly. See demo video.
- Be sure to prevent light noise on camera: Avoid Light Noise
- More camera info
Servo/Steering
- You will need to put the car into demo mode 1 and connect up a terminal to the serial port in order to get feedback on the values for your particular servo as hooked up. Then change
MAX_STEER_LEFTandMAX_STEER_RIGHTinSpices.cppwith these values to tell the program how your servo is hooked up.
Speed Control
- This program does not have proper speed control but does modify the speed slightly based on recent error results from the camera. It also modifies the differential speed to each wheel to have better control around curves.
- While debugging your car you may want to lower the speed. See the constants
LIGHT_SPEEDandLUDICROUS_SPEEDinSpices.cpp. There you can change the speeds used for Modes 4 and 5 above. Range of valid values are 0.4-0.7. Lower is better when debugging the car around the track (mainly to minimize crash forces!).
Strange Gotchas
Glitchy Motors
- Apparently there is contention between TPM0_CH0 and OpenSDA micro that causes strange issues with Motors (PWM interference). This will cause glitches in motor activty when hooked up to USB only: Found contention
More Info
- The Freescale Cup main info page
- redxeth youtube channel with useful related videos\
Spices/Spices.cpp@2:999ca57934bf, 2014-06-25 (annotated)
- Committer:
- simonchow
- Date:
- Wed Jun 25 18:25:18 2014 +0000
- Revision:
- 2:999ca57934bf
- Parent:
- 1:87b28e8b9941
Track mode: Speed controlled with pot0; Kp controlled with pot1;
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 |
| redxeth | 0:98e98e01a6ce | 11 | #define MAX_LINE_WIDTH 15 |
| 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 |
| redxeth | 1:87b28e8b9941 | 18 | #define MAX_STEER_LEFT -0.42 // value determined by demo mode 1 measure (have to be adjusted with every servo horn attach) |
| redxeth | 1:87b28e8b9941 | 19 | #define MAX_STEER_RIGHT 0.43 // 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_KP 0.008 |
| redxeth | 0:98e98e01a6ce | 27 | #define TUNE_KI 0 |
| redxeth | 0:98e98e01a6ce | 28 | #define TUNE_KD 0 |
| redxeth | 0:98e98e01a6ce | 29 | #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 | 1:87b28e8b9941 | 30 | #define SPEED_ADJUST 4 // do not change |
| redxeth | 0:98e98e01a6ce | 31 | #define ABS_ERROR_THRESH 10 // number of pixels line position offset before changing KP value |
| simonchow | 2:999ca57934bf | 32 | #define CONTROL_METHOD 1 // which control method to use |
| redxeth | 0:98e98e01a6ce | 33 | |
| redxeth | 0:98e98e01a6ce | 34 | |
| redxeth | 0:98e98e01a6ce | 35 | // Drive/motor params |
| redxeth | 1:87b28e8b9941 | 36 | #define LIGHT_SPEED 0.4 // easy speed |
| redxeth | 1:87b28e8b9941 | 37 | #define LUDICROUS_SPEED 0.6 // faster speed |
| redxeth | 0:98e98e01a6ce | 38 | #define MAX_POWER 100 // percent max power (for speed adjustments) |
| redxeth | 0:98e98e01a6ce | 39 | |
| redxeth | 0:98e98e01a6ce | 40 | // algo params |
| redxeth | 0:98e98e01a6ce | 41 | #define UNKNOWN_COUNT_MAX 50 // max value to allow for unknown track conditions before killing engine |
| redxeth | 0:98e98e01a6ce | 42 | #define STARTGATEFOUNDMAX 0 // max value to allow for finding starting gate before killing engine |
| redxeth | 0:98e98e01a6ce | 43 | #define STARTGATEDELAY 50 // Delay before searching for starting gate to kill engine |
| redxeth | 0:98e98e01a6ce | 44 | |
| redxeth | 0:98e98e01a6ce | 45 | |
| redxeth | 0:98e98e01a6ce | 46 | // image processing vars |
| redxeth | 0:98e98e01a6ce | 47 | uint16_t GrabLineScanImage0[NUM_LINE_SCAN]; // snapshot of camera data for this 'frame' |
| redxeth | 0:98e98e01a6ce | 48 | float DerivLineScanImage0[NUM_LINE_SCAN]; // derivative of line scan data |
| redxeth | 0:98e98e01a6ce | 49 | float NegEdges[NUM_LINE_SCAN]; // array-- set of where in line scan data negative edges found |
| redxeth | 0:98e98e01a6ce | 50 | float PosEdges[NUM_LINE_SCAN]; // array-- set of where in line scan data positive edges found |
| redxeth | 0:98e98e01a6ce | 51 | 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 | 52 | uint16_t MaxLightIntensity = 0; // max measured light intensity -- to account for lighting differences |
| redxeth | 0:98e98e01a6ce | 53 | uint16_t MinLightIntensity = (1 << 12); // min measured light intensity -- to account for lighting differences |
| redxeth | 0:98e98e01a6ce | 54 | float maxDerVal = 0; // max deriv value |
| redxeth | 0:98e98e01a6ce | 55 | float minDerVal = (float) (1 << 12); // min deriv value |
| redxeth | 0:98e98e01a6ce | 56 | float aveDerVal = 0; // average deriv value |
| redxeth | 0:98e98e01a6ce | 57 | float DerivThreshold = (1 << 9); // Derivative Threshold (default) |
| redxeth | 0:98e98e01a6ce | 58 | float PosDerivThreshold = (1 << 9); // Pos Edge Derivative Threshold (default) |
| redxeth | 0:98e98e01a6ce | 59 | float NegDerivThreshold = (1 << 9); // Neg Edge Derivative Threshold (default) |
| redxeth | 0:98e98e01a6ce | 60 | |
| redxeth | 0:98e98e01a6ce | 61 | |
| redxeth | 0:98e98e01a6ce | 62 | // Steering control variables |
| redxeth | 0:98e98e01a6ce | 63 | float CurrentLinePosition; // Current position of track line (in pixels -- 0 to 127) |
| redxeth | 0:98e98e01a6ce | 64 | float LastLinePosition; // Last position of track line (in pixels -- 0 to 127) |
| redxeth | 0:98e98e01a6ce | 65 | float CurrentLinePosError = 0; // Current line position error (used for derivative calc) |
| redxeth | 0:98e98e01a6ce | 66 | float AbsError; |
| redxeth | 0:98e98e01a6ce | 67 | float LastLinePosError = 0; // Last line position error (used for derivative calc) |
| redxeth | 0:98e98e01a6ce | 68 | float SumLinePosError = 0; // Sum of line position error (used for integral calc) |
| redxeth | 0:98e98e01a6ce | 69 | float DerivError = 0; // Derivative of error |
| redxeth | 0:98e98e01a6ce | 70 | float CurrentSteerSetting = (MAX_STEER_RIGHT + MAX_STEER_LEFT) / 2; // drive straight at first |
| redxeth | 0:98e98e01a6ce | 71 | float CurrentLeftDriveSetting = 0; // Drive setting (left wheel) |
| redxeth | 0:98e98e01a6ce | 72 | float CurrentRightDriveSetting = 0; // Drive setting (right wheel) |
| redxeth | 0:98e98e01a6ce | 73 | |
| redxeth | 0:98e98e01a6ce | 74 | // Speed control vars |
| redxeth | 1:87b28e8b9941 | 75 | float MaxSpeed; // maximum speed allowed |
| redxeth | 0:98e98e01a6ce | 76 | |
| redxeth | 0:98e98e01a6ce | 77 | uint16_t startRaceTicker; // ticker at start of race1 |
| redxeth | 0:98e98e01a6ce | 78 | |
| redxeth | 0:98e98e01a6ce | 79 | // Custom Data Types |
| redxeth | 0:98e98e01a6ce | 80 | typedef enum TrackStatusType {Unknown, |
| redxeth | 0:98e98e01a6ce | 81 | LineFound, |
| redxeth | 0:98e98e01a6ce | 82 | StartGateFound, |
| redxeth | 0:98e98e01a6ce | 83 | LineJustLeft} TrackStatusType; |
| redxeth | 0:98e98e01a6ce | 84 | |
| redxeth | 0:98e98e01a6ce | 85 | TrackStatusType CurrentTrackStatus; // current track status |
| redxeth | 0:98e98e01a6ce | 86 | TrackStatusType LastTrackStatus; // last track status |
| redxeth | 0:98e98e01a6ce | 87 | |
| redxeth | 0:98e98e01a6ce | 88 | /* typedef enum TrackType {NotSure, |
| redxeth | 0:98e98e01a6ce | 89 | Straight, |
| redxeth | 0:98e98e01a6ce | 90 | Curve, |
| redxeth | 0:98e98e01a6ce | 91 | Wiggle, |
| redxeth | 0:98e98e01a6ce | 92 | Bumps, |
| redxeth | 0:98e98e01a6ce | 93 | StartGate, |
| redxeth | 0:98e98e01a6ce | 94 | UpHill, |
| redxeth | 0:98e98e01a6ce | 95 | DownHill} TrackType; |
| redxeth | 0:98e98e01a6ce | 96 | |
| redxeth | 0:98e98e01a6ce | 97 | TrackType CurrentTrack; */ |
| redxeth | 0:98e98e01a6ce | 98 | |
| redxeth | 0:98e98e01a6ce | 99 | |
| redxeth | 0:98e98e01a6ce | 100 | struct LogData { |
| redxeth | 0:98e98e01a6ce | 101 | float linepos; |
| redxeth | 0:98e98e01a6ce | 102 | float steersetting; |
| redxeth | 0:98e98e01a6ce | 103 | float leftdrivesetting; |
| redxeth | 0:98e98e01a6ce | 104 | float rightdrivesetting; |
| redxeth | 0:98e98e01a6ce | 105 | }; |
| redxeth | 0:98e98e01a6ce | 106 | |
| redxeth | 0:98e98e01a6ce | 107 | LogData frameLogs[NUM_LOG_FRAMES]; // array of log data to store |
| redxeth | 0:98e98e01a6ce | 108 | int logDataIndex; // index for log data |
| redxeth | 0:98e98e01a6ce | 109 | |
| redxeth | 0:98e98e01a6ce | 110 | |
| redxeth | 0:98e98e01a6ce | 111 | int StartGateFoundCount = 0; // how many times start gate has been found |
| redxeth | 0:98e98e01a6ce | 112 | int UnknownCount = 0; // how many times nothing has been found (to help with kill switch implementation) |
| redxeth | 0:98e98e01a6ce | 113 | bool go = false; // Car can go! Should be set to false to start. |
| redxeth | 0:98e98e01a6ce | 114 | |
| redxeth | 0:98e98e01a6ce | 115 | // EXTRA CONTROL PARAMETERS |
| redxeth | 0:98e98e01a6ce | 116 | bool debugFakeMode = false; // if true, ignores real camera and uses fake camera input instead; used for data processing debug |
| redxeth | 1:87b28e8b9941 | 117 | bool terminalOutput = false; // whether output terminal data |
| redxeth | 0:98e98e01a6ce | 118 | bool doLogData = false; // whether to capture log data to output later on |
| redxeth | 1:87b28e8b9941 | 119 | bool killSwitch = false; // whether to enable Kill Switch (allow engine to stop after not finding track) |
| redxeth | 0:98e98e01a6ce | 120 | bool startGateStop = false; // whether to stop or not depending on starting gate reading |
| redxeth | 0:98e98e01a6ce | 121 | bool doRisky = false; // race style-- whether conservative or risky |
| redxeth | 0:98e98e01a6ce | 122 | |
| redxeth | 1:87b28e8b9941 | 123 | void TrackMode() |
| redxeth | 1:87b28e8b9941 | 124 | { |
| redxeth | 1:87b28e8b9941 | 125 | // set mode based on DIP switches |
| redxeth | 1:87b28e8b9941 | 126 | useMode(); |
| redxeth | 0:98e98e01a6ce | 127 | |
| redxeth | 1:87b28e8b9941 | 128 | // grab Terminal Mode based on DIP switch 4 |
| redxeth | 1:87b28e8b9941 | 129 | terminalOutput = terminalMode(); |
| redxeth | 0:98e98e01a6ce | 130 | |
| redxeth | 1:87b28e8b9941 | 131 | // Every 2s (or Terminal Output is off, i.e. race mode!) |
| redxeth | 0:98e98e01a6ce | 132 | // AND line scan image ready (or fake mode where image is always ready) |
| redxeth | 1:87b28e8b9941 | 133 | // (ticker updates every 10ms) (Line scan image ready every 10ms) |
| redxeth | 1:87b28e8b9941 | 134 | if((TFC_Ticker[0]>2000 || (!terminalOutput)) && (TFC_LineScanImageReady>0 || debugFakeMode)) |
| redxeth | 0:98e98e01a6ce | 135 | { |
| redxeth | 0:98e98e01a6ce | 136 | |
| redxeth | 0:98e98e01a6ce | 137 | // stuff that needs to be reset with each image frame |
| redxeth | 0:98e98e01a6ce | 138 | TFC_Ticker[0] = 0; |
| redxeth | 0:98e98e01a6ce | 139 | TFC_LineScanImageReady=0; // must reset to 0 after detecting non-zero |
| redxeth | 0:98e98e01a6ce | 140 | MaxLightIntensity = 0; // reset |
| redxeth | 0:98e98e01a6ce | 141 | MinLightIntensity = (1 << 12); // reset |
| redxeth | 0:98e98e01a6ce | 142 | |
| redxeth | 0:98e98e01a6ce | 143 | // grab camera frame |
| redxeth | 0:98e98e01a6ce | 144 | grabCameraFrame(); |
| redxeth | 0:98e98e01a6ce | 145 | |
| redxeth | 0:98e98e01a6ce | 146 | // calcalate derivative of linescandata, filter starttime data |
| redxeth | 0:98e98e01a6ce | 147 | derivativeLineScan(&GrabLineScanImage0[0], &DerivLineScanImage0[0]); |
| redxeth | 0:98e98e01a6ce | 148 | |
| redxeth | 0:98e98e01a6ce | 149 | |
| redxeth | 0:98e98e01a6ce | 150 | // adjust deriv threshold based on max lighting value |
| redxeth | 0:98e98e01a6ce | 151 | // has to be called before find edges |
| redxeth | 0:98e98e01a6ce | 152 | adjustLights(); |
| redxeth | 0:98e98e01a6ce | 153 | |
| redxeth | 0:98e98e01a6ce | 154 | //find edges from derivative data |
| redxeth | 0:98e98e01a6ce | 155 | findEdges_v2(&DerivLineScanImage0[0]); |
| redxeth | 1:87b28e8b9941 | 156 | |
| redxeth | 0:98e98e01a6ce | 157 | //review edge data and set position or starting flag appropriately |
| redxeth | 0:98e98e01a6ce | 158 | reviewEdges(); |
| redxeth | 1:87b28e8b9941 | 159 | |
| redxeth | 1:87b28e8b9941 | 160 | // print terminal data if desired (slows down things) |
| redxeth | 1:87b28e8b9941 | 161 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 162 | printLineScanData(&GrabLineScanImage0[0]); |
| redxeth | 0:98e98e01a6ce | 163 | printDerivLineScanData(&DerivLineScanImage0[0]); |
| redxeth | 0:98e98e01a6ce | 164 | printAdjustLightsData(); |
| redxeth | 0:98e98e01a6ce | 165 | printEdgesFound(); |
| redxeth | 0:98e98e01a6ce | 166 | } |
| redxeth | 0:98e98e01a6ce | 167 | |
| redxeth | 0:98e98e01a6ce | 168 | // ** Track Status available at this point ** |
| redxeth | 0:98e98e01a6ce | 169 | |
| redxeth | 0:98e98e01a6ce | 170 | // Update things based on latest track status |
| redxeth | 0:98e98e01a6ce | 171 | // e.g. change steering setting, stop car, ... |
| redxeth | 0:98e98e01a6ce | 172 | ActOnTrackStatus(); |
| redxeth | 0:98e98e01a6ce | 173 | |
| redxeth | 0:98e98e01a6ce | 174 | //give LED feedback as to track status |
| redxeth | 0:98e98e01a6ce | 175 | feedbackLights(); |
| redxeth | 1:87b28e8b9941 | 176 | |
| redxeth | 0:98e98e01a6ce | 177 | // control max power (speed) |
| redxeth | 0:98e98e01a6ce | 178 | SpeedControl(); |
| redxeth | 0:98e98e01a6ce | 179 | |
| redxeth | 0:98e98e01a6ce | 180 | // Drive!! |
| redxeth | 0:98e98e01a6ce | 181 | Drive(); |
| redxeth | 0:98e98e01a6ce | 182 | |
| redxeth | 1:87b28e8b9941 | 183 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 184 | TERMINAL_PRINTF("\r\n**************************END********************************\r\n"); |
| redxeth | 0:98e98e01a6ce | 185 | } |
| redxeth | 0:98e98e01a6ce | 186 | |
| redxeth | 0:98e98e01a6ce | 187 | } |
| redxeth | 0:98e98e01a6ce | 188 | } |
| redxeth | 0:98e98e01a6ce | 189 | |
| redxeth | 1:87b28e8b9941 | 190 | uint16_t getMode(){ |
| redxeth | 1:87b28e8b9941 | 191 | // get mode based on first 3 DIP switches (1, 2, 3) |
| redxeth | 1:87b28e8b9941 | 192 | return (TFC_GetDIP_Switch()&0x07); |
| redxeth | 0:98e98e01a6ce | 193 | } |
| redxeth | 0:98e98e01a6ce | 194 | |
| redxeth | 1:87b28e8b9941 | 195 | void useMode() |
| redxeth | 0:98e98e01a6ce | 196 | { |
| redxeth | 0:98e98e01a6ce | 197 | |
| redxeth | 1:87b28e8b9941 | 198 | switch(getMode()) |
| redxeth | 1:87b28e8b9941 | 199 | { |
| redxeth | 1:87b28e8b9941 | 200 | default: |
| redxeth | 1:87b28e8b9941 | 201 | case 4 : // Track Mode, Auto Steer, safe settings |
| redxeth | 1:87b28e8b9941 | 202 | doRisky = false; |
| redxeth | 1:87b28e8b9941 | 203 | break; |
| redxeth | 1:87b28e8b9941 | 204 | |
| redxeth | 1:87b28e8b9941 | 205 | case 5 : // Track Mode, Auto Steer, fast settings |
| redxeth | 1:87b28e8b9941 | 206 | doRisky = true; |
| redxeth | 1:87b28e8b9941 | 207 | break; |
| redxeth | 0:98e98e01a6ce | 208 | |
| redxeth | 1:87b28e8b9941 | 209 | case 6 : |
| redxeth | 1:87b28e8b9941 | 210 | // NOT USED YET |
| redxeth | 1:87b28e8b9941 | 211 | break; |
| redxeth | 1:87b28e8b9941 | 212 | |
| redxeth | 1:87b28e8b9941 | 213 | case 7 : |
| redxeth | 1:87b28e8b9941 | 214 | // NOT USED YET |
| redxeth | 1:87b28e8b9941 | 215 | break; |
| redxeth | 1:87b28e8b9941 | 216 | |
| redxeth | 1:87b28e8b9941 | 217 | } // end case |
| redxeth | 1:87b28e8b9941 | 218 | |
| redxeth | 1:87b28e8b9941 | 219 | if(TFC_GetDIP_Switch()&0x08) // SWITCH 4 Terminal Output on/off |
| redxeth | 0:98e98e01a6ce | 220 | startGateStop = true; |
| redxeth | 0:98e98e01a6ce | 221 | else |
| redxeth | 0:98e98e01a6ce | 222 | startGateStop = false; |
| redxeth | 0:98e98e01a6ce | 223 | |
| redxeth | 1:87b28e8b9941 | 224 | } |
| redxeth | 0:98e98e01a6ce | 225 | |
| redxeth | 1:87b28e8b9941 | 226 | bool terminalMode() |
| redxeth | 1:87b28e8b9941 | 227 | { |
| redxeth | 1:87b28e8b9941 | 228 | return ((TFC_GetDIP_Switch()>>3)&0x01 == 1); |
| redxeth | 0:98e98e01a6ce | 229 | } |
| redxeth | 0:98e98e01a6ce | 230 | |
| redxeth | 0:98e98e01a6ce | 231 | void grabCameraFrame() |
| redxeth | 0:98e98e01a6ce | 232 | { |
| redxeth | 0:98e98e01a6ce | 233 | uint32_t i = 0; |
| redxeth | 0:98e98e01a6ce | 234 | uint8_t fake_type = 4; // type of fake data if used |
| redxeth | 0:98e98e01a6ce | 235 | |
| redxeth | 0:98e98e01a6ce | 236 | for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data (128) from Camera 0 |
| redxeth | 0:98e98e01a6ce | 237 | { |
| redxeth | 0:98e98e01a6ce | 238 | |
| redxeth | 0:98e98e01a6ce | 239 | if (debugFakeMode) { // use fake camera data |
| redxeth | 0:98e98e01a6ce | 240 | switch (fake_type) { |
| redxeth | 0:98e98e01a6ce | 241 | case 0: // ideal track -- line in center |
| redxeth | 0:98e98e01a6ce | 242 | if (i<57 || i > 70) |
| redxeth | 0:98e98e01a6ce | 243 | GrabLineScanImage0[i] = 0xFFF; // no line |
| redxeth | 0:98e98e01a6ce | 244 | else |
| redxeth | 0:98e98e01a6ce | 245 | GrabLineScanImage0[i] = 0x4B0; // line |
| redxeth | 0:98e98e01a6ce | 246 | break; |
| redxeth | 0:98e98e01a6ce | 247 | case 1: // ideal track -- line to the left |
| redxeth | 0:98e98e01a6ce | 248 | if (i<27 || i > 40) |
| redxeth | 0:98e98e01a6ce | 249 | GrabLineScanImage0[i] = 0xFFF; // no line |
| redxeth | 0:98e98e01a6ce | 250 | else |
| redxeth | 0:98e98e01a6ce | 251 | GrabLineScanImage0[i] = 0x4B0; // line |
| redxeth | 0:98e98e01a6ce | 252 | break; |
| redxeth | 0:98e98e01a6ce | 253 | case 2: // ideal track -- line to the right |
| redxeth | 0:98e98e01a6ce | 254 | if (i<87 || i > 100) |
| redxeth | 0:98e98e01a6ce | 255 | GrabLineScanImage0[i] = 0xFFF; // no line |
| redxeth | 0:98e98e01a6ce | 256 | else |
| redxeth | 0:98e98e01a6ce | 257 | GrabLineScanImage0[i] = 0x4B0; // line |
| redxeth | 0:98e98e01a6ce | 258 | break; |
| redxeth | 0:98e98e01a6ce | 259 | case 3: // ideal track -- starting gate! |
| redxeth | 0:98e98e01a6ce | 260 | // TBD |
| redxeth | 0:98e98e01a6ce | 261 | break; |
| redxeth | 0:98e98e01a6ce | 262 | case 4: // less than ideal track -- debug multi-edge issue! |
| redxeth | 0:98e98e01a6ce | 263 | if (i<54) |
| redxeth | 0:98e98e01a6ce | 264 | GrabLineScanImage0[i] = 4000; // no line |
| redxeth | 0:98e98e01a6ce | 265 | if (i == 54) |
| redxeth | 0:98e98e01a6ce | 266 | GrabLineScanImage0[i] = 3370; // neg edge |
| redxeth | 0:98e98e01a6ce | 267 | if (i == 55) |
| redxeth | 0:98e98e01a6ce | 268 | GrabLineScanImage0[i] = 3309; // neg edge |
| redxeth | 0:98e98e01a6ce | 269 | if (i == 56) |
| redxeth | 0:98e98e01a6ce | 270 | GrabLineScanImage0[i] = 2016; // neg edge |
| redxeth | 0:98e98e01a6ce | 271 | if (i == 57) |
| redxeth | 0:98e98e01a6ce | 272 | GrabLineScanImage0[i] = 711; // neg edge |
| redxeth | 0:98e98e01a6ce | 273 | if (i == 58) |
| redxeth | 0:98e98e01a6ce | 274 | GrabLineScanImage0[i] = 696; // neg edge |
| redxeth | 0:98e98e01a6ce | 275 | if ((i>58) && (i<69)) |
| redxeth | 0:98e98e01a6ce | 276 | GrabLineScanImage0[i] = 500; // line |
| redxeth | 0:98e98e01a6ce | 277 | if (i == 69) |
| redxeth | 0:98e98e01a6ce | 278 | GrabLineScanImage0[i] = 1800; // pos edge |
| redxeth | 0:98e98e01a6ce | 279 | if (i > 69) |
| redxeth | 0:98e98e01a6ce | 280 | GrabLineScanImage0[i] = 4000; // no line |
| redxeth | 0:98e98e01a6ce | 281 | default: |
| redxeth | 0:98e98e01a6ce | 282 | break; |
| redxeth | 0:98e98e01a6ce | 283 | } |
| redxeth | 0:98e98e01a6ce | 284 | |
| redxeth | 0:98e98e01a6ce | 285 | } else { // use real camera data |
| redxeth | 0:98e98e01a6ce | 286 | GrabLineScanImage0[i] = TFC_LineScanImage0[i]; |
| redxeth | 0:98e98e01a6ce | 287 | } |
| redxeth | 0:98e98e01a6ce | 288 | } |
| redxeth | 0:98e98e01a6ce | 289 | |
| redxeth | 0:98e98e01a6ce | 290 | |
| redxeth | 0:98e98e01a6ce | 291 | } |
| redxeth | 0:98e98e01a6ce | 292 | |
| redxeth | 0:98e98e01a6ce | 293 | void printLineScanData(uint16_t* LineScanData) |
| redxeth | 0:98e98e01a6ce | 294 | { |
| redxeth | 0:98e98e01a6ce | 295 | uint32_t i = 0; |
| redxeth | 0:98e98e01a6ce | 296 | float Val; |
| redxeth | 0:98e98e01a6ce | 297 | |
| redxeth | 0:98e98e01a6ce | 298 | TERMINAL_PRINTF("LINE SCAN DATA:,"); |
| redxeth | 0:98e98e01a6ce | 299 | |
| redxeth | 0:98e98e01a6ce | 300 | for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data (128) from Camera 0 |
| redxeth | 0:98e98e01a6ce | 301 | { |
| redxeth | 0:98e98e01a6ce | 302 | if (1 == 1) { // use float to print |
| redxeth | 0:98e98e01a6ce | 303 | Val = (float) LineScanData[i]; |
| redxeth | 0:98e98e01a6ce | 304 | TERMINAL_PRINTF("%9.3f",Val); |
| redxeth | 0:98e98e01a6ce | 305 | if(i==MAX_LINE_SCAN) // when last data reached put in line return |
| redxeth | 0:98e98e01a6ce | 306 | TERMINAL_PRINTF("\r\n"); |
| redxeth | 0:98e98e01a6ce | 307 | else |
| redxeth | 0:98e98e01a6ce | 308 | TERMINAL_PRINTF(","); |
| redxeth | 0:98e98e01a6ce | 309 | } else { |
| redxeth | 0:98e98e01a6ce | 310 | TERMINAL_PRINTF("0x%X",LineScanData[i]); |
| redxeth | 0:98e98e01a6ce | 311 | if(i==MAX_LINE_SCAN) // when last data reached put in line return |
| redxeth | 0:98e98e01a6ce | 312 | TERMINAL_PRINTF("\r\n",LineScanData[i]); |
| redxeth | 0:98e98e01a6ce | 313 | else |
| redxeth | 0:98e98e01a6ce | 314 | TERMINAL_PRINTF(",",LineScanData[i]); |
| redxeth | 0:98e98e01a6ce | 315 | } |
| redxeth | 0:98e98e01a6ce | 316 | } |
| redxeth | 0:98e98e01a6ce | 317 | |
| redxeth | 0:98e98e01a6ce | 318 | } |
| redxeth | 0:98e98e01a6ce | 319 | |
| redxeth | 0:98e98e01a6ce | 320 | void printDerivLineScanData(float* derivLineScanData) |
| redxeth | 0:98e98e01a6ce | 321 | { |
| redxeth | 0:98e98e01a6ce | 322 | uint32_t i, minCnt = 0, maxCnt = 0; |
| redxeth | 0:98e98e01a6ce | 323 | |
| redxeth | 0:98e98e01a6ce | 324 | minCnt = FILTER_ENDS; |
| redxeth | 0:98e98e01a6ce | 325 | maxCnt = NUM_LINE_SCAN - FILTER_ENDS; |
| redxeth | 0:98e98e01a6ce | 326 | |
| redxeth | 0:98e98e01a6ce | 327 | TERMINAL_PRINTF("DERIVATIVE DATA:,"); |
| redxeth | 0:98e98e01a6ce | 328 | |
| redxeth | 0:98e98e01a6ce | 329 | for(i=minCnt;i<maxCnt;i++) // print one line worth of data (128) from Camera 0 |
| redxeth | 0:98e98e01a6ce | 330 | { |
| redxeth | 0:98e98e01a6ce | 331 | TERMINAL_PRINTF("%9.3f",derivLineScanData[i]); |
| redxeth | 0:98e98e01a6ce | 332 | if(i==maxCnt-1) // when last data reached put in line return |
| redxeth | 0:98e98e01a6ce | 333 | TERMINAL_PRINTF("\r\n",derivLineScanData[i]); |
| redxeth | 0:98e98e01a6ce | 334 | else |
| redxeth | 0:98e98e01a6ce | 335 | TERMINAL_PRINTF(", ",derivLineScanData[i]); |
| redxeth | 0:98e98e01a6ce | 336 | } |
| redxeth | 0:98e98e01a6ce | 337 | |
| redxeth | 0:98e98e01a6ce | 338 | } |
| redxeth | 0:98e98e01a6ce | 339 | |
| redxeth | 0:98e98e01a6ce | 340 | void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut) |
| redxeth | 0:98e98e01a6ce | 341 | { |
| redxeth | 0:98e98e01a6ce | 342 | |
| redxeth | 0:98e98e01a6ce | 343 | uint32_t i, minCnt = 0, maxCnt = 0; |
| redxeth | 0:98e98e01a6ce | 344 | float DerVal, upperDerVal, lowerDerVal = 0; |
| redxeth | 0:98e98e01a6ce | 345 | |
| redxeth | 0:98e98e01a6ce | 346 | maxDerVal = 0; |
| redxeth | 0:98e98e01a6ce | 347 | minDerVal = (float) (1 << 12); |
| redxeth | 0:98e98e01a6ce | 348 | aveDerVal = 0; |
| redxeth | 0:98e98e01a6ce | 349 | |
| redxeth | 0:98e98e01a6ce | 350 | minCnt = FILTER_ENDS; |
| redxeth | 0:98e98e01a6ce | 351 | maxCnt = NUM_LINE_SCAN - FILTER_ENDS; |
| redxeth | 0:98e98e01a6ce | 352 | |
| redxeth | 0:98e98e01a6ce | 353 | // TERMINAL_PRINTF("i, upperDerVal, lowerDerVal, DerVal\r\n"); |
| redxeth | 0:98e98e01a6ce | 354 | |
| redxeth | 0:98e98e01a6ce | 355 | for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0 |
| redxeth | 0:98e98e01a6ce | 356 | { |
| redxeth | 0:98e98e01a6ce | 357 | |
| redxeth | 0:98e98e01a6ce | 358 | // store max light intensity value |
| redxeth | 0:98e98e01a6ce | 359 | if (LineScanDataIn[i] > MaxLightIntensity) |
| redxeth | 0:98e98e01a6ce | 360 | MaxLightIntensity = LineScanDataIn[i]; |
| redxeth | 0:98e98e01a6ce | 361 | |
| redxeth | 0:98e98e01a6ce | 362 | // store min light intensity value |
| redxeth | 0:98e98e01a6ce | 363 | if (LineScanDataIn[i] < MinLightIntensity) |
| redxeth | 0:98e98e01a6ce | 364 | MinLightIntensity = LineScanDataIn[i]; |
| redxeth | 0:98e98e01a6ce | 365 | |
| redxeth | 0:98e98e01a6ce | 366 | |
| redxeth | 0:98e98e01a6ce | 367 | // Central Derivative |
| redxeth | 0:98e98e01a6ce | 368 | if (i==minCnt) { // start point |
| redxeth | 0:98e98e01a6ce | 369 | upperDerVal = (float)(LineScanDataIn[i+1]); |
| redxeth | 0:98e98e01a6ce | 370 | lowerDerVal = (float)(LineScanDataIn[i]); // make same as start point |
| redxeth | 0:98e98e01a6ce | 371 | } else if (i==maxCnt - 1){ // end point |
| redxeth | 0:98e98e01a6ce | 372 | upperDerVal = (float)(LineScanDataIn[i]); // make same as end point |
| redxeth | 0:98e98e01a6ce | 373 | lowerDerVal = (float)(LineScanDataIn[i-1]); |
| redxeth | 0:98e98e01a6ce | 374 | } else { // any other point |
| redxeth | 0:98e98e01a6ce | 375 | upperDerVal = (float)(LineScanDataIn[i+1]); |
| redxeth | 0:98e98e01a6ce | 376 | lowerDerVal = (float)(LineScanDataIn[i-1]); |
| redxeth | 0:98e98e01a6ce | 377 | } |
| redxeth | 0:98e98e01a6ce | 378 | DerVal = (upperDerVal - lowerDerVal) / 2; |
| redxeth | 0:98e98e01a6ce | 379 | // TERMINAL_PRINTF("%d,%9.3f,%9.3f,%9.3f\r\n", i, upperDerVal, lowerDerVal, DerVal); |
| redxeth | 0:98e98e01a6ce | 380 | |
| redxeth | 0:98e98e01a6ce | 381 | if (DerVal > maxDerVal) { |
| redxeth | 0:98e98e01a6ce | 382 | maxDerVal = DerVal; |
| redxeth | 0:98e98e01a6ce | 383 | } |
| redxeth | 0:98e98e01a6ce | 384 | if (DerVal < minDerVal) { |
| redxeth | 0:98e98e01a6ce | 385 | minDerVal = DerVal; |
| redxeth | 0:98e98e01a6ce | 386 | } |
| redxeth | 0:98e98e01a6ce | 387 | aveDerVal = aveDerVal + DerVal; //get sum |
| redxeth | 0:98e98e01a6ce | 388 | DerivLineScanDataOut[i] = DerVal; |
| redxeth | 0:98e98e01a6ce | 389 | } |
| redxeth | 0:98e98e01a6ce | 390 | aveDerVal = (float) aveDerVal / (maxCnt - minCnt); |
| redxeth | 0:98e98e01a6ce | 391 | } |
| redxeth | 0:98e98e01a6ce | 392 | |
| redxeth | 0:98e98e01a6ce | 393 | //Not reliable for finding edges! |
| redxeth | 0:98e98e01a6ce | 394 | void findEdges(float* derivLineScanData) |
| redxeth | 0:98e98e01a6ce | 395 | { |
| redxeth | 0:98e98e01a6ce | 396 | // search for edges in deriviative data using a threshold |
| redxeth | 0:98e98e01a6ce | 397 | // need to store in a hash if that's possible... |
| redxeth | 0:98e98e01a6ce | 398 | // combine edges that are a pixel apart |
| redxeth | 0:98e98e01a6ce | 399 | |
| redxeth | 0:98e98e01a6ce | 400 | int i, minCnt = 0, maxCnt = 0; |
| redxeth | 0:98e98e01a6ce | 401 | int multiNegEdgeCnt = 1, multiNegEdgeSum = 0; |
| redxeth | 0:98e98e01a6ce | 402 | int multiPosEdgeCnt = 1, multiPosEdgeSum = 0; |
| redxeth | 0:98e98e01a6ce | 403 | |
| redxeth | 0:98e98e01a6ce | 404 | minCnt = FILTER_ENDS; |
| redxeth | 0:98e98e01a6ce | 405 | maxCnt = NUM_LINE_SCAN - FILTER_ENDS; |
| redxeth | 0:98e98e01a6ce | 406 | |
| redxeth | 0:98e98e01a6ce | 407 | numNegEdges = 0; |
| redxeth | 0:98e98e01a6ce | 408 | numPosEdges = 0; |
| redxeth | 0:98e98e01a6ce | 409 | for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0 |
| redxeth | 0:98e98e01a6ce | 410 | { |
| redxeth | 0:98e98e01a6ce | 411 | if (derivLineScanData[i] <= NegDerivThreshold) // NEGATIVE EDGE FOUND! |
| redxeth | 0:98e98e01a6ce | 412 | { |
| redxeth | 1:87b28e8b9941 | 413 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 414 | TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); |
| redxeth | 0:98e98e01a6ce | 415 | } |
| redxeth | 0:98e98e01a6ce | 416 | |
| redxeth | 0:98e98e01a6ce | 417 | if ((numNegEdges > 0) && (NegEdges[numNegEdges - 1] + 1 == i )) // if no multi edges |
| redxeth | 0:98e98e01a6ce | 418 | { // edge actually across multiple pixels |
| redxeth | 0:98e98e01a6ce | 419 | multiNegEdgeCnt++; |
| redxeth | 0:98e98e01a6ce | 420 | multiNegEdgeSum = multiNegEdgeSum + i; |
| redxeth | 1:87b28e8b9941 | 421 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 422 | TERMINAL_PRINTF("MULTIEDGE FOUND! MultiNegEdgeCnt: %d; MultiNegEdgeSum: %d\r\n", multiNegEdgeCnt, multiNegEdgeSum); |
| redxeth | 0:98e98e01a6ce | 423 | } |
| redxeth | 0:98e98e01a6ce | 424 | } else { // not a multi-pixel edge known at this time, store negative edge index value |
| redxeth | 0:98e98e01a6ce | 425 | numNegEdges++; |
| redxeth | 1:87b28e8b9941 | 426 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 427 | TERMINAL_PRINTF("NEG EDGE STORED WITH INDEX %d. NUM NEG EDGES = %d\r\n", i, numNegEdges); |
| redxeth | 0:98e98e01a6ce | 428 | } |
| redxeth | 0:98e98e01a6ce | 429 | NegEdges[numNegEdges - 1] = (float) i; |
| redxeth | 0:98e98e01a6ce | 430 | multiNegEdgeSum = i; |
| redxeth | 0:98e98e01a6ce | 431 | } |
| redxeth | 0:98e98e01a6ce | 432 | |
| redxeth | 0:98e98e01a6ce | 433 | |
| redxeth | 0:98e98e01a6ce | 434 | } else if (derivLineScanData[i] > PosDerivThreshold) { // POSITIVE EDGE FOUND! |
| redxeth | 0:98e98e01a6ce | 435 | |
| redxeth | 1:87b28e8b9941 | 436 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 437 | TERMINAL_PRINTF("POS EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); |
| redxeth | 0:98e98e01a6ce | 438 | } |
| redxeth | 0:98e98e01a6ce | 439 | |
| redxeth | 0:98e98e01a6ce | 440 | if ((numPosEdges > 0) && (PosEdges[numPosEdges - 1] + 1 == i )) |
| redxeth | 0:98e98e01a6ce | 441 | { // edge actually across multiple pixels |
| redxeth | 0:98e98e01a6ce | 442 | multiPosEdgeCnt++; |
| redxeth | 0:98e98e01a6ce | 443 | multiPosEdgeSum = multiPosEdgeSum + i; |
| redxeth | 1:87b28e8b9941 | 444 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 445 | TERMINAL_PRINTF("MULTIEDGE FOUND! MultiPosEdgeCnt: %d; MultiPosEdgeSum: %d\r\n", multiPosEdgeCnt, multiPosEdgeSum); |
| redxeth | 0:98e98e01a6ce | 446 | } |
| redxeth | 0:98e98e01a6ce | 447 | } else { // not a multi-pixel edge known at this time, store Posative edge index value |
| redxeth | 1:87b28e8b9941 | 448 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 449 | TERMINAL_PRINTF("POS EDGE STORED WITH INDEX %d. NUM POS EDGES = %d\r\n", i, numPosEdges); |
| redxeth | 0:98e98e01a6ce | 450 | } |
| redxeth | 0:98e98e01a6ce | 451 | numPosEdges++; |
| redxeth | 0:98e98e01a6ce | 452 | PosEdges[numPosEdges - 1] = (float) i; |
| redxeth | 0:98e98e01a6ce | 453 | multiPosEdgeSum = i; |
| redxeth | 0:98e98e01a6ce | 454 | } |
| redxeth | 0:98e98e01a6ce | 455 | |
| redxeth | 0:98e98e01a6ce | 456 | } else { // NO EDGE FOUND |
| redxeth | 0:98e98e01a6ce | 457 | // combine multi-edges if there are any |
| redxeth | 0:98e98e01a6ce | 458 | if (multiNegEdgeCnt > 1) |
| redxeth | 0:98e98e01a6ce | 459 | { |
| redxeth | 0:98e98e01a6ce | 460 | NegEdges[numNegEdges - 1] = (float) multiNegEdgeSum / multiNegEdgeCnt; |
| redxeth | 0:98e98e01a6ce | 461 | multiNegEdgeCnt = 1; multiNegEdgeSum = 0; |
| redxeth | 0:98e98e01a6ce | 462 | } |
| redxeth | 0:98e98e01a6ce | 463 | if (multiPosEdgeCnt > 1) |
| redxeth | 0:98e98e01a6ce | 464 | { |
| redxeth | 0:98e98e01a6ce | 465 | PosEdges[numPosEdges - 1] = (float) multiPosEdgeSum / multiPosEdgeCnt; |
| redxeth | 0:98e98e01a6ce | 466 | multiPosEdgeCnt = 1; multiPosEdgeSum = 0; |
| redxeth | 0:98e98e01a6ce | 467 | } |
| redxeth | 0:98e98e01a6ce | 468 | |
| redxeth | 0:98e98e01a6ce | 469 | } |
| redxeth | 0:98e98e01a6ce | 470 | } |
| redxeth | 0:98e98e01a6ce | 471 | |
| redxeth | 0:98e98e01a6ce | 472 | } |
| redxeth | 0:98e98e01a6ce | 473 | |
| redxeth | 0:98e98e01a6ce | 474 | |
| redxeth | 0:98e98e01a6ce | 475 | void findEdges_v2(float* derivLineScanData) |
| redxeth | 0:98e98e01a6ce | 476 | { |
| redxeth | 0:98e98e01a6ce | 477 | // search for edges in deriviative data using a threshold |
| redxeth | 0:98e98e01a6ce | 478 | // need to store in a hash if that's possible... |
| redxeth | 0:98e98e01a6ce | 479 | // combine edges that are a pixel apart |
| redxeth | 0:98e98e01a6ce | 480 | |
| redxeth | 0:98e98e01a6ce | 481 | int i; |
| redxeth | 0:98e98e01a6ce | 482 | |
| redxeth | 0:98e98e01a6ce | 483 | int NegEdgeBufCnt = 0, NegEdgeBufSum = 0; // serves as buffer to store neg edges found next to each other |
| redxeth | 0:98e98e01a6ce | 484 | int PosEdgeBufCnt = 0, PosEdgeBufSum = 0; // serves as buffer to store pos edges found next to each other |
| redxeth | 0:98e98e01a6ce | 485 | |
| redxeth | 0:98e98e01a6ce | 486 | int minCnt = FILTER_ENDS; |
| redxeth | 0:98e98e01a6ce | 487 | int maxCnt = NUM_LINE_SCAN - FILTER_ENDS; |
| redxeth | 0:98e98e01a6ce | 488 | |
| redxeth | 0:98e98e01a6ce | 489 | |
| redxeth | 0:98e98e01a6ce | 490 | |
| redxeth | 0:98e98e01a6ce | 491 | numNegEdges = 0; // count of neg edges found thus far |
| redxeth | 0:98e98e01a6ce | 492 | numPosEdges = 0; // count of pos edges found thus far |
| redxeth | 0:98e98e01a6ce | 493 | for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0 |
| redxeth | 0:98e98e01a6ce | 494 | { |
| redxeth | 0:98e98e01a6ce | 495 | |
| redxeth | 0:98e98e01a6ce | 496 | if (derivLineScanData[i] <= NegDerivThreshold) // NEGATIVE EDGE FOUND! |
| redxeth | 0:98e98e01a6ce | 497 | { |
| redxeth | 0:98e98e01a6ce | 498 | |
| redxeth | 1:87b28e8b9941 | 499 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 500 | TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); |
| redxeth | 0:98e98e01a6ce | 501 | } |
| redxeth | 0:98e98e01a6ce | 502 | |
| redxeth | 0:98e98e01a6ce | 503 | NegEdgeBufCnt++; // add value to neg edge buffer |
| redxeth | 0:98e98e01a6ce | 504 | NegEdgeBufSum = NegEdgeBufSum + i; |
| redxeth | 0:98e98e01a6ce | 505 | |
| redxeth | 0:98e98e01a6ce | 506 | } else if (derivLineScanData[i] > PosDerivThreshold) { // POSITIVE EDGE FOUND! |
| redxeth | 0:98e98e01a6ce | 507 | |
| redxeth | 1:87b28e8b9941 | 508 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 509 | TERMINAL_PRINTF("POS EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]); |
| redxeth | 0:98e98e01a6ce | 510 | } |
| redxeth | 0:98e98e01a6ce | 511 | |
| redxeth | 0:98e98e01a6ce | 512 | PosEdgeBufCnt++; // add value to pos edge buffer |
| redxeth | 0:98e98e01a6ce | 513 | PosEdgeBufSum = PosEdgeBufSum + i; |
| redxeth | 0:98e98e01a6ce | 514 | |
| redxeth | 0:98e98e01a6ce | 515 | } else { // NO EDGE FOUND |
| redxeth | 0:98e98e01a6ce | 516 | |
| redxeth | 0:98e98e01a6ce | 517 | // POP EDGE BUFFERS IF NON-EMPTY AND STORE TO EDGE "STACK" (i.e. edges found) |
| redxeth | 0:98e98e01a6ce | 518 | |
| redxeth | 0:98e98e01a6ce | 519 | if (NegEdgeBufCnt > 0) { |
| redxeth | 0:98e98e01a6ce | 520 | // store edge value |
| redxeth | 0:98e98e01a6ce | 521 | numNegEdges++; |
| redxeth | 0:98e98e01a6ce | 522 | NegEdges[numNegEdges - 1] = (float) NegEdgeBufSum / NegEdgeBufCnt; |
| redxeth | 0:98e98e01a6ce | 523 | |
| redxeth | 0:98e98e01a6ce | 524 | // clear edge buffer |
| redxeth | 0:98e98e01a6ce | 525 | NegEdgeBufSum = 0; NegEdgeBufCnt = 0; |
| redxeth | 0:98e98e01a6ce | 526 | } |
| redxeth | 0:98e98e01a6ce | 527 | |
| redxeth | 0:98e98e01a6ce | 528 | if (PosEdgeBufCnt > 0) { |
| redxeth | 0:98e98e01a6ce | 529 | // store edge value |
| redxeth | 0:98e98e01a6ce | 530 | numPosEdges++; |
| redxeth | 0:98e98e01a6ce | 531 | PosEdges[numPosEdges - 1] = (float) PosEdgeBufSum / PosEdgeBufCnt; |
| redxeth | 0:98e98e01a6ce | 532 | |
| redxeth | 0:98e98e01a6ce | 533 | // clear edge buffer |
| redxeth | 0:98e98e01a6ce | 534 | PosEdgeBufSum = 0; PosEdgeBufCnt = 0; |
| redxeth | 0:98e98e01a6ce | 535 | } |
| redxeth | 0:98e98e01a6ce | 536 | |
| redxeth | 0:98e98e01a6ce | 537 | } |
| redxeth | 0:98e98e01a6ce | 538 | |
| redxeth | 0:98e98e01a6ce | 539 | } |
| redxeth | 0:98e98e01a6ce | 540 | |
| redxeth | 0:98e98e01a6ce | 541 | } |
| redxeth | 0:98e98e01a6ce | 542 | |
| redxeth | 0:98e98e01a6ce | 543 | void printEdgesFound() |
| redxeth | 0:98e98e01a6ce | 544 | { |
| redxeth | 0:98e98e01a6ce | 545 | int i; |
| redxeth | 0:98e98e01a6ce | 546 | |
| redxeth | 0:98e98e01a6ce | 547 | // Check that neg edges captured ok |
| redxeth | 0:98e98e01a6ce | 548 | TERMINAL_PRINTF("NEGATIVE EDGES FOUND:,"); |
| redxeth | 0:98e98e01a6ce | 549 | for(i=0;i<=numNegEdges-1;i++) |
| redxeth | 0:98e98e01a6ce | 550 | { |
| redxeth | 0:98e98e01a6ce | 551 | TERMINAL_PRINTF("%9.3f",NegEdges[i]); |
| redxeth | 0:98e98e01a6ce | 552 | if(i==numNegEdges-1) // when last data reached put in line return |
| redxeth | 0:98e98e01a6ce | 553 | TERMINAL_PRINTF("\r\n"); |
| redxeth | 0:98e98e01a6ce | 554 | else |
| redxeth | 0:98e98e01a6ce | 555 | TERMINAL_PRINTF(", "); |
| redxeth | 0:98e98e01a6ce | 556 | } |
| redxeth | 0:98e98e01a6ce | 557 | |
| redxeth | 0:98e98e01a6ce | 558 | |
| redxeth | 0:98e98e01a6ce | 559 | // Check that pos edges captured ok |
| redxeth | 0:98e98e01a6ce | 560 | TERMINAL_PRINTF("POSITIVE EDGES FOUND:,"); |
| redxeth | 0:98e98e01a6ce | 561 | for(i=0;i<=numPosEdges-1;i++) |
| redxeth | 0:98e98e01a6ce | 562 | { |
| redxeth | 0:98e98e01a6ce | 563 | TERMINAL_PRINTF("%9.3f",PosEdges[i]); |
| redxeth | 0:98e98e01a6ce | 564 | if(i==numPosEdges-1) // when last data reached put in line return |
| redxeth | 0:98e98e01a6ce | 565 | TERMINAL_PRINTF("\r\n"); |
| redxeth | 0:98e98e01a6ce | 566 | else |
| redxeth | 0:98e98e01a6ce | 567 | TERMINAL_PRINTF(", "); |
| redxeth | 0:98e98e01a6ce | 568 | } |
| redxeth | 0:98e98e01a6ce | 569 | |
| redxeth | 0:98e98e01a6ce | 570 | } |
| redxeth | 0:98e98e01a6ce | 571 | |
| redxeth | 0:98e98e01a6ce | 572 | void reviewEdges() |
| redxeth | 0:98e98e01a6ce | 573 | { |
| redxeth | 0:98e98e01a6ce | 574 | LastTrackStatus = CurrentTrackStatus; |
| redxeth | 0:98e98e01a6ce | 575 | |
| redxeth | 0:98e98e01a6ce | 576 | if ((numPosEdges == 1) && (numNegEdges == 1)) // only one negative and positive edge found (LINE) |
| redxeth | 0:98e98e01a6ce | 577 | { |
| redxeth | 0:98e98e01a6ce | 578 | if (((PosEdges[0] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[0] - NegEdges[0]) <= MAX_LINE_WIDTH)) // has proper expected width |
| redxeth | 0:98e98e01a6ce | 579 | { |
| redxeth | 0:98e98e01a6ce | 580 | CurrentTrackStatus = LineFound; // report line found! |
| redxeth | 0:98e98e01a6ce | 581 | UnknownCount = 0; // reset unknown status count |
| redxeth | 0:98e98e01a6ce | 582 | LastLinePosition = CurrentLinePosition; |
| redxeth | 0:98e98e01a6ce | 583 | CurrentLinePosition = (PosEdges[0]+NegEdges[0]) / 2; // update line position |
| redxeth | 0:98e98e01a6ce | 584 | } |
| redxeth | 0:98e98e01a6ce | 585 | } else if ((numPosEdges == 1) && (numNegEdges == 0)) { // 1 pos edge found (POSSIBLE LINE) |
| redxeth | 0:98e98e01a6ce | 586 | if ((PosEdges[0] <= MAX_LINE_WIDTH) && (LastLinePosError < 0)) // pos edge is within line width of edge of camera (LEFT) |
| redxeth | 0:98e98e01a6ce | 587 | { |
| redxeth | 0:98e98e01a6ce | 588 | CurrentTrackStatus = LineFound; // report line found! |
| redxeth | 0:98e98e01a6ce | 589 | UnknownCount = 0; // reset unknown status count |
| redxeth | 0:98e98e01a6ce | 590 | LastLinePosition = CurrentLinePosition; |
| redxeth | 0:98e98e01a6ce | 591 | CurrentLinePosition = PosEdges[0] - ( MAX_LINE_WIDTH / 2); // update line position |
| redxeth | 0:98e98e01a6ce | 592 | // TERMINAL_PRINTF("*** SINGLE POSEDGE LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); |
| redxeth | 0:98e98e01a6ce | 593 | } |
| redxeth | 0:98e98e01a6ce | 594 | } else if ((numNegEdges == 1) && (numPosEdges == 0)) { // 1 neg edge found (POSSIBLE LINE) |
| redxeth | 0:98e98e01a6ce | 595 | if ((NegEdges[0] >= (MAX_LINE_SCAN - MAX_LINE_WIDTH)) && (LastLinePosError > 0)) // neg edge is within line width of edge of camera (RIGHT) |
| redxeth | 0:98e98e01a6ce | 596 | { |
| redxeth | 0:98e98e01a6ce | 597 | CurrentTrackStatus = LineFound; // report line found! |
| redxeth | 0:98e98e01a6ce | 598 | UnknownCount = 0; // reset unknown status count |
| redxeth | 0:98e98e01a6ce | 599 | LastLinePosition = CurrentLinePosition; |
| redxeth | 0:98e98e01a6ce | 600 | CurrentLinePosition = NegEdges[0] + ( MAX_LINE_WIDTH / 2); // update line position |
| redxeth | 0:98e98e01a6ce | 601 | // TERMINAL_PRINTF("*** SINGLE NEGEDGE LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); |
| redxeth | 0:98e98e01a6ce | 602 | } |
| redxeth | 0:98e98e01a6ce | 603 | } else if ((numPosEdges == 2) && (numNegEdges == 2)) { // 2 negative and 2 positive edges found (STARTING/FINISH GATE) |
| redxeth | 0:98e98e01a6ce | 604 | |
| redxeth | 0:98e98e01a6ce | 605 | if ((((NegEdges[0] - PosEdges[0]) >= MIN_LINE_WIDTH) && ((NegEdges[0] - PosEdges[0]) <= MAX_LINE_WIDTH)) && // white left 'line' |
| redxeth | 0:98e98e01a6ce | 606 | (((NegEdges[1] - PosEdges[1]) >= MIN_LINE_WIDTH) && ((NegEdges[1] - PosEdges[1]) <= MAX_LINE_WIDTH)) && // white right 'line' |
| redxeth | 0:98e98e01a6ce | 607 | (((PosEdges[1] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[1] - NegEdges[0]) <= MAX_LINE_WIDTH)) // actual track line |
| redxeth | 0:98e98e01a6ce | 608 | ) |
| redxeth | 0:98e98e01a6ce | 609 | |
| redxeth | 0:98e98e01a6ce | 610 | |
| redxeth | 0:98e98e01a6ce | 611 | if (startRaceTicker > STARTGATEDELAY) { // only start counting for starting gate until after delay |
| redxeth | 0:98e98e01a6ce | 612 | StartGateFoundCount++; |
| redxeth | 0:98e98e01a6ce | 613 | } |
| redxeth | 0:98e98e01a6ce | 614 | |
| redxeth | 0:98e98e01a6ce | 615 | CurrentTrackStatus = StartGateFound; |
| redxeth | 0:98e98e01a6ce | 616 | UnknownCount = 0; // reset unknown status count |
| redxeth | 0:98e98e01a6ce | 617 | |
| redxeth | 0:98e98e01a6ce | 618 | } else if ((numPosEdges > 1) && (numNegEdges > 1)) { // more than 1 negative edge and positive edge found (but not 2 for both) (STARTING / FINISH GATE) |
| redxeth | 0:98e98e01a6ce | 619 | |
| redxeth | 0:98e98e01a6ce | 620 | // remove edges that aren't close to center TBD DDHH |
| redxeth | 0:98e98e01a6ce | 621 | |
| redxeth | 1:87b28e8b9941 | 622 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 623 | TERMINAL_PRINTF("***************************************** \r\n"); |
| redxeth | 0:98e98e01a6ce | 624 | TERMINAL_PRINTF("********** NOT SURE FOUND ********** \r\n"); |
| redxeth | 0:98e98e01a6ce | 625 | TERMINAL_PRINTF("***************************************** \r\n"); |
| redxeth | 0:98e98e01a6ce | 626 | } |
| redxeth | 0:98e98e01a6ce | 627 | CurrentTrackStatus = Unknown; |
| redxeth | 0:98e98e01a6ce | 628 | |
| redxeth | 0:98e98e01a6ce | 629 | } else { // no track or starting gate found |
| redxeth | 0:98e98e01a6ce | 630 | |
| redxeth | 1:87b28e8b9941 | 631 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 632 | TERMINAL_PRINTF("***************************************** \r\n"); |
| redxeth | 0:98e98e01a6ce | 633 | TERMINAL_PRINTF("*** !!!!!!!!!! LINE NOT FOUND !!!!!!! *** \r\n", CurrentLinePosition); |
| redxeth | 0:98e98e01a6ce | 634 | TERMINAL_PRINTF("***************************************** \r\n"); |
| redxeth | 0:98e98e01a6ce | 635 | } |
| redxeth | 0:98e98e01a6ce | 636 | |
| redxeth | 0:98e98e01a6ce | 637 | CurrentTrackStatus = Unknown; |
| redxeth | 0:98e98e01a6ce | 638 | UnknownCount++; |
| redxeth | 0:98e98e01a6ce | 639 | } |
| redxeth | 0:98e98e01a6ce | 640 | |
| redxeth | 0:98e98e01a6ce | 641 | |
| redxeth | 0:98e98e01a6ce | 642 | |
| redxeth | 0:98e98e01a6ce | 643 | |
| redxeth | 0:98e98e01a6ce | 644 | } |
| redxeth | 0:98e98e01a6ce | 645 | |
| redxeth | 0:98e98e01a6ce | 646 | void ActOnTrackStatus() |
| redxeth | 0:98e98e01a6ce | 647 | { |
| redxeth | 0:98e98e01a6ce | 648 | // Decide what to do next based on current track status |
| redxeth | 0:98e98e01a6ce | 649 | |
| redxeth | 0:98e98e01a6ce | 650 | if (CurrentTrackStatus == LineFound) { // LINE FOUND! |
| redxeth | 0:98e98e01a6ce | 651 | |
| redxeth | 1:87b28e8b9941 | 652 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 653 | TERMINAL_PRINTF("***************************************** \r\n"); |
| redxeth | 0:98e98e01a6ce | 654 | TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); |
| redxeth | 0:98e98e01a6ce | 655 | TERMINAL_PRINTF("***************************************** \r\n"); |
| redxeth | 0:98e98e01a6ce | 656 | } |
| redxeth | 0:98e98e01a6ce | 657 | |
| redxeth | 0:98e98e01a6ce | 658 | // Update steering position |
| redxeth | 0:98e98e01a6ce | 659 | SteeringControl(); |
| redxeth | 0:98e98e01a6ce | 660 | |
| redxeth | 0:98e98e01a6ce | 661 | // Apply to servo |
| redxeth | 0:98e98e01a6ce | 662 | Steer(); |
| redxeth | 0:98e98e01a6ce | 663 | |
| redxeth | 0:98e98e01a6ce | 664 | } else if (CurrentTrackStatus == StartGateFound) { // STARTING GATE FOUND |
| redxeth | 0:98e98e01a6ce | 665 | |
| redxeth | 1:87b28e8b9941 | 666 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 667 | TERMINAL_PRINTF("***************************************** \r\n"); |
| redxeth | 0:98e98e01a6ce | 668 | TERMINAL_PRINTF("********** STARTING GATE FOUND ********** \r\n"); |
| redxeth | 0:98e98e01a6ce | 669 | TERMINAL_PRINTF("********** count = %d ********** \r\n", StartGateFoundCount); |
| redxeth | 0:98e98e01a6ce | 670 | TERMINAL_PRINTF("***************************************** \r\n"); |
| redxeth | 0:98e98e01a6ce | 671 | } |
| redxeth | 0:98e98e01a6ce | 672 | |
| redxeth | 0:98e98e01a6ce | 673 | // END RACE! |
| redxeth | 0:98e98e01a6ce | 674 | if (startGateStop) { |
| redxeth | 0:98e98e01a6ce | 675 | if (StartGateFoundCount > STARTGATEFOUNDMAX) |
| redxeth | 0:98e98e01a6ce | 676 | { |
| redxeth | 0:98e98e01a6ce | 677 | go = false; // STOP!! |
| redxeth | 0:98e98e01a6ce | 678 | } |
| redxeth | 0:98e98e01a6ce | 679 | } |
| redxeth | 0:98e98e01a6ce | 680 | |
| redxeth | 0:98e98e01a6ce | 681 | } |
| redxeth | 0:98e98e01a6ce | 682 | |
| redxeth | 0:98e98e01a6ce | 683 | |
| redxeth | 0:98e98e01a6ce | 684 | |
| redxeth | 0:98e98e01a6ce | 685 | } |
| redxeth | 0:98e98e01a6ce | 686 | |
| redxeth | 0:98e98e01a6ce | 687 | void SteeringControl() |
| redxeth | 0:98e98e01a6ce | 688 | { |
| simonchow | 2:999ca57934bf | 689 | float ReadPot1 = 1; |
| redxeth | 0:98e98e01a6ce | 690 | float targetPosition = (float)( (NUM_LINE_SCAN / 2) - 0.5); // target to achieve for line position |
| redxeth | 0:98e98e01a6ce | 691 | |
| redxeth | 0:98e98e01a6ce | 692 | float KP; // proportional control factor |
| redxeth | 0:98e98e01a6ce | 693 | float KI; // integral control factor |
| redxeth | 0:98e98e01a6ce | 694 | float KD; // derivative control factor |
| redxeth | 0:98e98e01a6ce | 695 | |
| redxeth | 0:98e98e01a6ce | 696 | float Pout, Iout, Dout; // PID terms |
| redxeth | 0:98e98e01a6ce | 697 | |
| redxeth | 0:98e98e01a6ce | 698 | // Calculate error |
| redxeth | 0:98e98e01a6ce | 699 | // make error to the right positive |
| redxeth | 0:98e98e01a6ce | 700 | // i.e. if LINE to the right-- then CurrentLinePosError > 0 |
| redxeth | 0:98e98e01a6ce | 701 | // if LINE to the left -- then CurrentLinePosError < 0 |
| redxeth | 0:98e98e01a6ce | 702 | CurrentLinePosError = CurrentLinePosition - targetPosition; |
| redxeth | 0:98e98e01a6ce | 703 | |
| redxeth | 0:98e98e01a6ce | 704 | // Get absolute error |
| redxeth | 0:98e98e01a6ce | 705 | if (CurrentLinePosError >= 0) |
| redxeth | 0:98e98e01a6ce | 706 | AbsError = CurrentLinePosError; |
| redxeth | 0:98e98e01a6ce | 707 | else |
| redxeth | 0:98e98e01a6ce | 708 | AbsError = -1 * CurrentLinePosError; |
| redxeth | 0:98e98e01a6ce | 709 | |
| redxeth | 0:98e98e01a6ce | 710 | // CHOOSE SET OF PID CONTROL PARAMETERS |
| redxeth | 0:98e98e01a6ce | 711 | switch (CONTROL_METHOD) { |
| redxeth | 0:98e98e01a6ce | 712 | case 0: |
| redxeth | 0:98e98e01a6ce | 713 | // Pure proportional control based on range of steering values vs. range of error values |
| redxeth | 0:98e98e01a6ce | 714 | KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT ) / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH ); |
| redxeth | 0:98e98e01a6ce | 715 | KD = 0; |
| redxeth | 0:98e98e01a6ce | 716 | KI = 0; |
| redxeth | 0:98e98e01a6ce | 717 | break; |
| redxeth | 0:98e98e01a6ce | 718 | case 1: |
| redxeth | 0:98e98e01a6ce | 719 | // Proportional control with 50% bit more oomph --- a bit more aggressive around the bends |
| simonchow | 2:999ca57934bf | 720 | ReadPot1 = TFC_ReadPot(1)+1; // pot range 0-2 |
| redxeth | 0:98e98e01a6ce | 721 | KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT ) / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH ); |
| simonchow | 2:999ca57934bf | 722 | KP = KP * ReadPot1; |
| redxeth | 0:98e98e01a6ce | 723 | KD = 0; |
| redxeth | 0:98e98e01a6ce | 724 | KI = 0; |
| redxeth | 0:98e98e01a6ce | 725 | break; |
| redxeth | 0:98e98e01a6ce | 726 | case 2: // MANUAL TUNING CASE 1 (use pot to help determine tuning parameters) |
| redxeth | 0:98e98e01a6ce | 727 | KP = TUNE_KP; |
| redxeth | 0:98e98e01a6ce | 728 | KI = TUNE_KI; |
| redxeth | 0:98e98e01a6ce | 729 | KD = TUNE_KD; |
| redxeth | 0:98e98e01a6ce | 730 | case 3: |
| redxeth | 0:98e98e01a6ce | 731 | if (AbsError < ABS_ERROR_THRESH) { |
| redxeth | 0:98e98e01a6ce | 732 | KP = 0.003; // when relatively straight, keep KP gain low |
| redxeth | 0:98e98e01a6ce | 733 | } else { |
| redxeth | 0:98e98e01a6ce | 734 | KP = 0.010; // when curve begins or off track, increase KP gain |
| redxeth | 0:98e98e01a6ce | 735 | } |
| redxeth | 0:98e98e01a6ce | 736 | KI = 0; |
| redxeth | 0:98e98e01a6ce | 737 | KD = 0; |
| redxeth | 0:98e98e01a6ce | 738 | |
| redxeth | 0:98e98e01a6ce | 739 | default: |
| redxeth | 0:98e98e01a6ce | 740 | break; |
| redxeth | 0:98e98e01a6ce | 741 | } |
| redxeth | 0:98e98e01a6ce | 742 | |
| redxeth | 0:98e98e01a6ce | 743 | /* Pseudocode |
| redxeth | 0:98e98e01a6ce | 744 | previous_error = 0 |
| redxeth | 0:98e98e01a6ce | 745 | integral = 0 |
| redxeth | 0:98e98e01a6ce | 746 | start: |
| redxeth | 0:98e98e01a6ce | 747 | error = setpoint - measured_value |
| redxeth | 0:98e98e01a6ce | 748 | integral = integral + error*dt |
| redxeth | 0:98e98e01a6ce | 749 | derivative = (error - previous_error)/dt |
| redxeth | 0:98e98e01a6ce | 750 | output = Kp*error + Ki*integral + Kd*derivative |
| redxeth | 0:98e98e01a6ce | 751 | previous_error = error |
| redxeth | 0:98e98e01a6ce | 752 | wait(dt) |
| redxeth | 0:98e98e01a6ce | 753 | goto start |
| redxeth | 0:98e98e01a6ce | 754 | */ |
| redxeth | 0:98e98e01a6ce | 755 | |
| redxeth | 0:98e98e01a6ce | 756 | |
| redxeth | 1:87b28e8b9941 | 757 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 758 | TERMINAL_PRINTF("KP = %6.4f\r\n", KP); |
| redxeth | 0:98e98e01a6ce | 759 | TERMINAL_PRINTF("TARGET %6.3f\r\n", targetPosition); |
| redxeth | 0:98e98e01a6ce | 760 | } |
| redxeth | 0:98e98e01a6ce | 761 | |
| redxeth | 0:98e98e01a6ce | 762 | |
| redxeth | 0:98e98e01a6ce | 763 | |
| redxeth | 0:98e98e01a6ce | 764 | // Update integral of error |
| redxeth | 0:98e98e01a6ce | 765 | // i.e. if LINE stays to the right, then SumLinePosError increases |
| redxeth | 0:98e98e01a6ce | 766 | // i.e. if LINE stays to the left, then SumLinePosError decreases |
| redxeth | 0:98e98e01a6ce | 767 | SumLinePosError = SumLinePosError + ( CurrentLinePosError * DT ); |
| redxeth | 0:98e98e01a6ce | 768 | |
| redxeth | 0:98e98e01a6ce | 769 | DerivError = (CurrentLinePosError - LastLinePosError) / DT; |
| redxeth | 0:98e98e01a6ce | 770 | |
| redxeth | 1:87b28e8b9941 | 771 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 772 | TERMINAL_PRINTF("CURRENT LINE POSITION %9.3f\r\n", CurrentLinePosition); |
| redxeth | 0:98e98e01a6ce | 773 | TERMINAL_PRINTF("CURRENT LINE POSITION ERROR %9.3f\r\n", CurrentLinePosError); |
| redxeth | 0:98e98e01a6ce | 774 | } |
| redxeth | 0:98e98e01a6ce | 775 | |
| redxeth | 0:98e98e01a6ce | 776 | // SECOND- calculate new servo position |
| redxeth | 0:98e98e01a6ce | 777 | |
| redxeth | 0:98e98e01a6ce | 778 | // proportional control term |
| redxeth | 0:98e98e01a6ce | 779 | Pout = KP * CurrentLinePosError; |
| redxeth | 0:98e98e01a6ce | 780 | |
| redxeth | 0:98e98e01a6ce | 781 | // integral control term |
| redxeth | 0:98e98e01a6ce | 782 | Iout = KI * SumLinePosError; |
| redxeth | 0:98e98e01a6ce | 783 | |
| redxeth | 0:98e98e01a6ce | 784 | // Derivative control term |
| redxeth | 0:98e98e01a6ce | 785 | Dout = KD * DerivError; |
| redxeth | 0:98e98e01a6ce | 786 | |
| redxeth | 1:87b28e8b9941 | 787 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 788 | TERMINAL_PRINTF("KP = %6.4f\r\n", KP); |
| redxeth | 0:98e98e01a6ce | 789 | TERMINAL_PRINTF("KI = %6.4f\r\n", KI); |
| redxeth | 0:98e98e01a6ce | 790 | TERMINAL_PRINTF("KD = %6.4f\r\n", KD); |
| redxeth | 0:98e98e01a6ce | 791 | TERMINAL_PRINTF("Pout = %6.4f\r\n", Pout); |
| redxeth | 0:98e98e01a6ce | 792 | TERMINAL_PRINTF("Iout = %6.4f\r\n", Iout); |
| redxeth | 0:98e98e01a6ce | 793 | TERMINAL_PRINTF("Dout = %6.4f\r\n", Dout); |
| redxeth | 0:98e98e01a6ce | 794 | } |
| redxeth | 0:98e98e01a6ce | 795 | |
| redxeth | 0:98e98e01a6ce | 796 | // Finally add offset to steering to account for non-centered servo mounting |
| redxeth | 0:98e98e01a6ce | 797 | // CurrentSteerSetting = Pout + Iout + Dout + ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 ); |
| redxeth | 0:98e98e01a6ce | 798 | CurrentSteerSetting = Pout + ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 ); |
| redxeth | 0:98e98e01a6ce | 799 | |
| redxeth | 0:98e98e01a6ce | 800 | // store for next cycle deriv calculation |
| redxeth | 0:98e98e01a6ce | 801 | LastLinePosError = CurrentLinePosError; |
| redxeth | 0:98e98e01a6ce | 802 | |
| redxeth | 0:98e98e01a6ce | 803 | // for tuning control algo only |
| redxeth | 0:98e98e01a6ce | 804 | if (1 == 0) { |
| redxeth | 0:98e98e01a6ce | 805 | TERMINAL_PRINTF("*** ******************************** \r\n"); |
| redxeth | 0:98e98e01a6ce | 806 | TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition); |
| redxeth | 0:98e98e01a6ce | 807 | TERMINAL_PRINTF("*** ERROR %9.3f *** \r\n", CurrentLinePosError); |
| redxeth | 0:98e98e01a6ce | 808 | TERMINAL_PRINTF("*** INTEGRAL ERROR %9.3f *** \r\n", SumLinePosError); |
| redxeth | 0:98e98e01a6ce | 809 | TERMINAL_PRINTF("*** DERIVATIVE ERROR %9.3f *** \r\n", DerivError); |
| redxeth | 0:98e98e01a6ce | 810 | TERMINAL_PRINTF("*** P STEER SETTING %9.3f *** \r\n", CurrentSteerSetting); |
| redxeth | 0:98e98e01a6ce | 811 | TERMINAL_PRINTF("*** PI STEER SETTING %9.3f *** \r\n", (CurrentSteerSetting + Iout)); |
| redxeth | 0:98e98e01a6ce | 812 | TERMINAL_PRINTF("*** ******************************** \r\n"); |
| redxeth | 0:98e98e01a6ce | 813 | wait_ms(1000); |
| redxeth | 0:98e98e01a6ce | 814 | } |
| redxeth | 0:98e98e01a6ce | 815 | |
| redxeth | 0:98e98e01a6ce | 816 | } |
| redxeth | 0:98e98e01a6ce | 817 | |
| redxeth | 0:98e98e01a6ce | 818 | void Steer() |
| redxeth | 0:98e98e01a6ce | 819 | { |
| redxeth | 0:98e98e01a6ce | 820 | |
| redxeth | 0:98e98e01a6ce | 821 | // make sure doesn't go beyond steering limits |
| redxeth | 0:98e98e01a6ce | 822 | if (CurrentSteerSetting > MAX_STEER_RIGHT) |
| redxeth | 0:98e98e01a6ce | 823 | { |
| redxeth | 0:98e98e01a6ce | 824 | CurrentSteerSetting = MAX_STEER_RIGHT; |
| redxeth | 0:98e98e01a6ce | 825 | } else if (CurrentSteerSetting < MAX_STEER_LEFT) |
| redxeth | 0:98e98e01a6ce | 826 | { |
| redxeth | 0:98e98e01a6ce | 827 | CurrentSteerSetting = MAX_STEER_LEFT; |
| redxeth | 0:98e98e01a6ce | 828 | } |
| redxeth | 0:98e98e01a6ce | 829 | |
| redxeth | 1:87b28e8b9941 | 830 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 831 | TERMINAL_PRINTF("APPLYING SERVO SETTING %5.3f\r\n", CurrentSteerSetting); |
| redxeth | 0:98e98e01a6ce | 832 | } |
| redxeth | 0:98e98e01a6ce | 833 | TFC_SetServo(0,CurrentSteerSetting); |
| redxeth | 0:98e98e01a6ce | 834 | |
| redxeth | 0:98e98e01a6ce | 835 | } |
| redxeth | 0:98e98e01a6ce | 836 | |
| redxeth | 0:98e98e01a6ce | 837 | void SpeedControl() |
| redxeth | 0:98e98e01a6ce | 838 | { |
| redxeth | 0:98e98e01a6ce | 839 | |
| redxeth | 0:98e98e01a6ce | 840 | // Get max speed setting from reading pot0 |
| redxeth | 0:98e98e01a6ce | 841 | // then adjust |
| redxeth | 0:98e98e01a6ce | 842 | |
| redxeth | 0:98e98e01a6ce | 843 | float ErrLimit; |
| redxeth | 0:98e98e01a6ce | 844 | float LeftDriveRatio, RightDriveRatio; |
| redxeth | 0:98e98e01a6ce | 845 | |
| redxeth | 1:87b28e8b9941 | 846 | // set maximum speed |
| redxeth | 1:87b28e8b9941 | 847 | if (doRisky) |
| simonchow | 2:999ca57934bf | 848 | MaxSpeed = LUDICROUS_SPEED * ((TFC_ReadPot(0)+1)/2.0); // faster |
| redxeth | 1:87b28e8b9941 | 849 | else |
| simonchow | 2:999ca57934bf | 850 | MaxSpeed = LIGHT_SPEED * ((TFC_ReadPot(0)+1)/2.0); // slower |
| redxeth | 0:98e98e01a6ce | 851 | |
| redxeth | 0:98e98e01a6ce | 852 | switch (SPEED_ADJUST) |
| redxeth | 0:98e98e01a6ce | 853 | { |
| redxeth | 0:98e98e01a6ce | 854 | case 0: |
| redxeth | 0:98e98e01a6ce | 855 | // SPEED ADJUST METHOD 0 |
| redxeth | 0:98e98e01a6ce | 856 | // no speed adjust |
| redxeth | 0:98e98e01a6ce | 857 | LeftDriveRatio = MAX_POWER; |
| redxeth | 0:98e98e01a6ce | 858 | RightDriveRatio = LeftDriveRatio; |
| redxeth | 0:98e98e01a6ce | 859 | case 1: |
| redxeth | 0:98e98e01a6ce | 860 | // SPEED ADJUST METHOD 1 |
| redxeth | 0:98e98e01a6ce | 861 | // High speed when error is low, low speed when error is high |
| redxeth | 0:98e98e01a6ce | 862 | // lower speed when more than third outside of center |
| redxeth | 0:98e98e01a6ce | 863 | ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.33; |
| redxeth | 0:98e98e01a6ce | 864 | if (AbsError > ErrLimit) { |
| redxeth | 0:98e98e01a6ce | 865 | LeftDriveRatio = MIN_POWER; |
| redxeth | 0:98e98e01a6ce | 866 | } else { |
| redxeth | 0:98e98e01a6ce | 867 | LeftDriveRatio = MAX_POWER; |
| redxeth | 0:98e98e01a6ce | 868 | } |
| redxeth | 0:98e98e01a6ce | 869 | RightDriveRatio = LeftDriveRatio; |
| redxeth | 0:98e98e01a6ce | 870 | break; |
| redxeth | 0:98e98e01a6ce | 871 | case 2: |
| redxeth | 0:98e98e01a6ce | 872 | // SPEED ADJUST METHOD 2 |
| redxeth | 0:98e98e01a6ce | 873 | // Have max/min speed adjust proportional to absolute value of line error |
| redxeth | 0:98e98e01a6ce | 874 | ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO; |
| redxeth | 0:98e98e01a6ce | 875 | LeftDriveRatio = MAX_POWER - ((MAX_POWER - MIN_POWER) * (AbsError / ErrLimit)); |
| redxeth | 0:98e98e01a6ce | 876 | RightDriveRatio = LeftDriveRatio; |
| redxeth | 0:98e98e01a6ce | 877 | break; |
| redxeth | 0:98e98e01a6ce | 878 | case 3: |
| redxeth | 0:98e98e01a6ce | 879 | // SPEED ADJUST METHOD 3 |
| redxeth | 0:98e98e01a6ce | 880 | // have wheel relative speed proportional to absolute value of line error |
| redxeth | 1:87b28e8b9941 | 881 | if (CurrentLinePosError > 0) { // heading right, slow right wheel down |
| redxeth | 0:98e98e01a6ce | 882 | LeftDriveRatio = MAX_POWER; |
| redxeth | 1:87b28e8b9941 | 883 | RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); |
| redxeth | 1:87b28e8b9941 | 884 | } else if (CurrentLinePosError < 0) { // heading left, slow left wheel down |
| redxeth | 1:87b28e8b9941 | 885 | LeftDriveRatio = MAX_POWER - (MIN_POWER - MAX_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); // note sign change due to error being negative |
| redxeth | 0:98e98e01a6ce | 886 | RightDriveRatio = MAX_POWER; |
| redxeth | 0:98e98e01a6ce | 887 | } else { |
| redxeth | 0:98e98e01a6ce | 888 | LeftDriveRatio = MAX_POWER; |
| redxeth | 0:98e98e01a6ce | 889 | RightDriveRatio = MAX_POWER; |
| redxeth | 0:98e98e01a6ce | 890 | } |
| redxeth | 0:98e98e01a6ce | 891 | break; |
| redxeth | 0:98e98e01a6ce | 892 | case 4: |
| redxeth | 0:98e98e01a6ce | 893 | // SPEED ADJUST METHOD 4 |
| redxeth | 0:98e98e01a6ce | 894 | // have wheel relative speed proportional to absolute value of line error |
| redxeth | 0:98e98e01a6ce | 895 | // only when above a certain error |
| redxeth | 0:98e98e01a6ce | 896 | ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.1; |
| redxeth | 1:87b28e8b9941 | 897 | if (CurrentLinePosError > ErrLimit) { // right turn-- slow right wheel down a bit |
| redxeth | 1:87b28e8b9941 | 898 | LeftDriveRatio = MAX_POWER; |
| redxeth | 0:98e98e01a6ce | 899 | RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); |
| redxeth | 1:87b28e8b9941 | 900 | } else if (CurrentLinePosError < (-1 * ErrLimit)) { // left turn-- slow left wheel down a bit |
| redxeth | 1:87b28e8b9941 | 901 | LeftDriveRatio = MAX_POWER - (MIN_POWER - MAX_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ); // note sign change due to error being negative |
| redxeth | 1:87b28e8b9941 | 902 | RightDriveRatio = MAX_POWER; |
| redxeth | 1:87b28e8b9941 | 903 | } else { // when in center drive full speed |
| redxeth | 0:98e98e01a6ce | 904 | LeftDriveRatio = MAX_POWER; |
| redxeth | 0:98e98e01a6ce | 905 | RightDriveRatio = MAX_POWER; |
| redxeth | 0:98e98e01a6ce | 906 | } |
| redxeth | 0:98e98e01a6ce | 907 | break; |
| redxeth | 0:98e98e01a6ce | 908 | case 5: |
| redxeth | 0:98e98e01a6ce | 909 | // SPEED ADJUST METHOD 5 |
| redxeth | 0:98e98e01a6ce | 910 | // High speed when error is low, low speed when error is high |
| redxeth | 0:98e98e01a6ce | 911 | // lower speed when more than third outside of center |
| redxeth | 0:98e98e01a6ce | 912 | ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.2; |
| redxeth | 0:98e98e01a6ce | 913 | if (AbsError > ErrLimit) { |
| redxeth | 0:98e98e01a6ce | 914 | LeftDriveRatio = MIN_POWER; |
| redxeth | 0:98e98e01a6ce | 915 | } else { |
| redxeth | 0:98e98e01a6ce | 916 | LeftDriveRatio = MAX_POWER; |
| redxeth | 0:98e98e01a6ce | 917 | } |
| redxeth | 0:98e98e01a6ce | 918 | RightDriveRatio = LeftDriveRatio; |
| redxeth | 0:98e98e01a6ce | 919 | break; |
| redxeth | 0:98e98e01a6ce | 920 | case 6: |
| redxeth | 0:98e98e01a6ce | 921 | // SPEED ADJUST METHOD 6 |
| redxeth | 0:98e98e01a6ce | 922 | // High speed when error is low, low speed when error is high |
| redxeth | 0:98e98e01a6ce | 923 | // lower speed when more than third outside of center |
| redxeth | 0:98e98e01a6ce | 924 | if (AbsError > ABS_ERROR_THRESH) { |
| redxeth | 0:98e98e01a6ce | 925 | LeftDriveRatio = MIN_POWER; |
| redxeth | 0:98e98e01a6ce | 926 | } else { |
| redxeth | 0:98e98e01a6ce | 927 | LeftDriveRatio = MAX_POWER; |
| redxeth | 0:98e98e01a6ce | 928 | } |
| redxeth | 0:98e98e01a6ce | 929 | RightDriveRatio = LeftDriveRatio; |
| redxeth | 0:98e98e01a6ce | 930 | break; |
| redxeth | 0:98e98e01a6ce | 931 | default: |
| redxeth | 0:98e98e01a6ce | 932 | break; |
| redxeth | 0:98e98e01a6ce | 933 | |
| redxeth | 0:98e98e01a6ce | 934 | } |
| redxeth | 0:98e98e01a6ce | 935 | // TBD-- add speed adjust based on Xaccel sensor! |
| redxeth | 0:98e98e01a6ce | 936 | |
| redxeth | 0:98e98e01a6ce | 937 | |
| redxeth | 0:98e98e01a6ce | 938 | // currently no control mechanism as don't have speed sensor |
| redxeth | 0:98e98e01a6ce | 939 | CurrentLeftDriveSetting = (float) (LeftDriveRatio / 100) * MaxSpeed; |
| redxeth | 0:98e98e01a6ce | 940 | CurrentRightDriveSetting = (float) (RightDriveRatio / 100) * MaxSpeed; |
| redxeth | 0:98e98e01a6ce | 941 | |
| redxeth | 0:98e98e01a6ce | 942 | |
| redxeth | 1:87b28e8b9941 | 943 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 944 | TERMINAL_PRINTF("Abs Error: %4.2f\r\n", AbsError); |
| redxeth | 0:98e98e01a6ce | 945 | TERMINAL_PRINTF("Error Limit: %4.2f\r\n", ErrLimit); |
| redxeth | 1:87b28e8b9941 | 946 | TERMINAL_PRINTF("MAX SPEED = %5.2f\r\n", MaxSpeed); |
| redxeth | 0:98e98e01a6ce | 947 | TERMINAL_PRINTF("Current Left Drive Setting: %5.2f\r\n", CurrentLeftDriveSetting); |
| redxeth | 0:98e98e01a6ce | 948 | TERMINAL_PRINTF("Current Right Drive Setting: %5.2f\r\n", CurrentRightDriveSetting); |
| redxeth | 0:98e98e01a6ce | 949 | } |
| redxeth | 0:98e98e01a6ce | 950 | |
| redxeth | 0:98e98e01a6ce | 951 | } |
| redxeth | 0:98e98e01a6ce | 952 | |
| redxeth | 0:98e98e01a6ce | 953 | void Drive() |
| redxeth | 0:98e98e01a6ce | 954 | { |
| redxeth | 0:98e98e01a6ce | 955 | |
| redxeth | 0:98e98e01a6ce | 956 | // START! |
| redxeth | 0:98e98e01a6ce | 957 | // if not going, go when button A is pressed |
| redxeth | 0:98e98e01a6ce | 958 | if (!go) { |
| redxeth | 0:98e98e01a6ce | 959 | if(TFC_PUSH_BUTTON_0_PRESSED) { |
| redxeth | 0:98e98e01a6ce | 960 | go = true; |
| redxeth | 0:98e98e01a6ce | 961 | UnknownCount = 0; |
| redxeth | 0:98e98e01a6ce | 962 | StartGateFoundCount = 0; |
| redxeth | 0:98e98e01a6ce | 963 | startRaceTicker = TFC_Ticker[0]; // keep track of start of race |
| redxeth | 0:98e98e01a6ce | 964 | logDataIndex = 0; // reset log data index |
| redxeth | 0:98e98e01a6ce | 965 | } |
| redxeth | 0:98e98e01a6ce | 966 | } |
| redxeth | 0:98e98e01a6ce | 967 | |
| redxeth | 0:98e98e01a6ce | 968 | // STOP! |
| redxeth | 0:98e98e01a6ce | 969 | // if going, stop when button A is pressed |
| redxeth | 0:98e98e01a6ce | 970 | if (go) { |
| redxeth | 0:98e98e01a6ce | 971 | if(TFC_PUSH_BUTTON_1_PRESSED) { |
| redxeth | 0:98e98e01a6ce | 972 | go = false; |
| redxeth | 0:98e98e01a6ce | 973 | StartGateFoundCount = 0; |
| redxeth | 0:98e98e01a6ce | 974 | } |
| redxeth | 0:98e98e01a6ce | 975 | } |
| redxeth | 0:98e98e01a6ce | 976 | |
| redxeth | 0:98e98e01a6ce | 977 | // EMERGENCY STOP! |
| redxeth | 0:98e98e01a6ce | 978 | // 'kill switch' to prevent crashes off-track |
| redxeth | 0:98e98e01a6ce | 979 | if (killSwitch) { |
| redxeth | 0:98e98e01a6ce | 980 | if (UnknownCount > UNKNOWN_COUNT_MAX) { // if track not found after certain time |
| redxeth | 0:98e98e01a6ce | 981 | go = false; // kill engine |
| redxeth | 0:98e98e01a6ce | 982 | StartGateFoundCount = 0; |
| redxeth | 0:98e98e01a6ce | 983 | } |
| redxeth | 0:98e98e01a6ce | 984 | } |
| redxeth | 0:98e98e01a6ce | 985 | |
| redxeth | 0:98e98e01a6ce | 986 | // **************** |
| redxeth | 0:98e98e01a6ce | 987 | |
| redxeth | 0:98e98e01a6ce | 988 | if (!go) { // stop! |
| redxeth | 0:98e98e01a6ce | 989 | TFC_SetMotorPWM(0,0); //Make sure motors are off |
| redxeth | 0:98e98e01a6ce | 990 | TFC_HBRIDGE_DISABLE; |
| redxeth | 0:98e98e01a6ce | 991 | } |
| redxeth | 0:98e98e01a6ce | 992 | |
| redxeth | 0:98e98e01a6ce | 993 | if (go) { // go! |
| redxeth | 0:98e98e01a6ce | 994 | TFC_HBRIDGE_ENABLE; |
| redxeth | 0:98e98e01a6ce | 995 | // motor A = right, motor B = left based on way it is mounted |
| redxeth | 0:98e98e01a6ce | 996 | TFC_SetMotorPWM(CurrentRightDriveSetting,CurrentLeftDriveSetting); |
| redxeth | 0:98e98e01a6ce | 997 | } |
| redxeth | 0:98e98e01a6ce | 998 | } |
| redxeth | 0:98e98e01a6ce | 999 | |
| redxeth | 0:98e98e01a6ce | 1000 | |
| redxeth | 0:98e98e01a6ce | 1001 | void adjustLights() |
| redxeth | 0:98e98e01a6ce | 1002 | { |
| redxeth | 0:98e98e01a6ce | 1003 | |
| redxeth | 0:98e98e01a6ce | 1004 | // LIGHT ADJUST METHOD 1 |
| redxeth | 0:98e98e01a6ce | 1005 | // threshold is 1/5 of light intensity 'range' |
| redxeth | 0:98e98e01a6ce | 1006 | if (1 == 0) { |
| redxeth | 0:98e98e01a6ce | 1007 | DerivThreshold = (float) (MaxLightIntensity - MinLightIntensity) / 5; |
| redxeth | 0:98e98e01a6ce | 1008 | NegDerivThreshold = (float) -1 * (DerivThreshold); |
| redxeth | 0:98e98e01a6ce | 1009 | PosDerivThreshold = (float) (DerivThreshold); |
| redxeth | 0:98e98e01a6ce | 1010 | } else { |
| redxeth | 0:98e98e01a6ce | 1011 | // LIGHT ADJUST METHOD 2 -- SEEMS TO WORK MUCH BETTER |
| redxeth | 0:98e98e01a6ce | 1012 | // pos edge threshold is half range of max deriv above aver derive |
| redxeth | 0:98e98e01a6ce | 1013 | // neg edge threshold is half range of min deriv above aver derive |
| redxeth | 0:98e98e01a6ce | 1014 | |
| redxeth | 0:98e98e01a6ce | 1015 | NegDerivThreshold = (float) (minDerVal - aveDerVal) * DER_RATIO; |
| redxeth | 0:98e98e01a6ce | 1016 | PosDerivThreshold = (float) (maxDerVal - aveDerVal) * DER_RATIO; |
| redxeth | 0:98e98e01a6ce | 1017 | |
| redxeth | 0:98e98e01a6ce | 1018 | } |
| redxeth | 0:98e98e01a6ce | 1019 | |
| redxeth | 0:98e98e01a6ce | 1020 | printAdjustLightsData(); |
| redxeth | 0:98e98e01a6ce | 1021 | |
| redxeth | 0:98e98e01a6ce | 1022 | } |
| redxeth | 0:98e98e01a6ce | 1023 | |
| redxeth | 0:98e98e01a6ce | 1024 | void printAdjustLightsData() |
| redxeth | 0:98e98e01a6ce | 1025 | { |
| redxeth | 1:87b28e8b9941 | 1026 | if (terminalOutput) { |
| redxeth | 0:98e98e01a6ce | 1027 | TERMINAL_PRINTF("Max Light Intensity: %4d\r\n", MaxLightIntensity); |
| redxeth | 0:98e98e01a6ce | 1028 | TERMINAL_PRINTF("Min Light Intensity: %4d\r\n", MinLightIntensity); |
| redxeth | 0:98e98e01a6ce | 1029 | TERMINAL_PRINTF("Deriv Threshold: %9.3f\r\n", DerivThreshold); |
| redxeth | 0:98e98e01a6ce | 1030 | } |
| redxeth | 0:98e98e01a6ce | 1031 | |
| redxeth | 0:98e98e01a6ce | 1032 | } |
| redxeth | 0:98e98e01a6ce | 1033 | |
| redxeth | 0:98e98e01a6ce | 1034 | void feedbackLights() |
| redxeth | 0:98e98e01a6ce | 1035 | { |
| redxeth | 0:98e98e01a6ce | 1036 | switch (CurrentTrackStatus) |
| redxeth | 0:98e98e01a6ce | 1037 | { |
| redxeth | 0:98e98e01a6ce | 1038 | case LineFound: |
| redxeth | 0:98e98e01a6ce | 1039 | TFC_BAT_LED0_OFF; |
| redxeth | 0:98e98e01a6ce | 1040 | TFC_BAT_LED1_ON; |
| redxeth | 0:98e98e01a6ce | 1041 | TFC_BAT_LED2_ON; |
| redxeth | 0:98e98e01a6ce | 1042 | TFC_BAT_LED3_OFF; |
| redxeth | 0:98e98e01a6ce | 1043 | break; |
| redxeth | 0:98e98e01a6ce | 1044 | case StartGateFound: |
| redxeth | 0:98e98e01a6ce | 1045 | TFC_BAT_LED0_ON; |
| redxeth | 0:98e98e01a6ce | 1046 | TFC_BAT_LED1_OFF; |
| redxeth | 0:98e98e01a6ce | 1047 | TFC_BAT_LED2_OFF; |
| redxeth | 0:98e98e01a6ce | 1048 | TFC_BAT_LED3_ON; |
| redxeth | 0:98e98e01a6ce | 1049 | break; |
| redxeth | 0:98e98e01a6ce | 1050 | default: |
| redxeth | 0:98e98e01a6ce | 1051 | TFC_BAT_LED0_OFF; |
| redxeth | 0:98e98e01a6ce | 1052 | TFC_BAT_LED1_OFF; |
| redxeth | 0:98e98e01a6ce | 1053 | TFC_BAT_LED2_OFF; |
| redxeth | 0:98e98e01a6ce | 1054 | TFC_BAT_LED3_OFF; |
| redxeth | 0:98e98e01a6ce | 1055 | } |
| redxeth | 0:98e98e01a6ce | 1056 | |
| redxeth | 0:98e98e01a6ce | 1057 | } |
| redxeth | 0:98e98e01a6ce | 1058 |
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Created: | 24 Jun 2014 |
| Imports: | 21 |
| Forks: | 0 |
| Commits: | 3 |
| Dependents: | 0 |
| Dependencies: | 1 |
| Followers: | 3 |
The code in this repository is MIT licensed.