Daniel Hadad / TFC-RACING-DEMO

This is an example program that actually allows the car to race using the FRDM-TFC library!

Dependencies:   FRDM-TFC

Overview

This program 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.

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 Hardware Controls

DIP SWITCH CONTROLS:

  • Switch 1: Controls whether running main racing program (ON) or Demo program (OFF). Function of other 3 switches depend on this first switch.

RACING MODE (Switch 1 = ON)

  • Switch 2: Logging frame camera data to RAM while racing. Enable (ON) or Disable (OFF).
  • Switch 3: Risky / faster racing (ON); Conservative / slower race option (OFF). NOTE: Faster mode may cause car to fall off track. Needs tweaking...
  • Switch 4: Enable Start Gate Stop (ON) or Disable (OFF). NOTE: May not actually stop at start gate. Needs tweaking...
  • POTs 1,2 - controls nothing at the moment
  • PUSHBUTTON A - START car race!
  • PUSHBUTTON B - END CAR RACE / (while holding down when 'log frame data' active will also output terminal data gathered during race)
  • LEDs 1,2 light when Track is found
  • LEDs 0,3 light when Start Gate is Found

DEMO MODE 0 (Switch 1 = OFF; Switch 2 = OFF, Switch 3 = OFF)

  • PUSHBUTTON A - Light LED 0
  • PUSHBUTTON B - Light LED 3

DEMO MODE 1 (Switch 1 = OFF; Switch 2 = ON, Switch 3 = OFF)

  • POT 1 - Controls Servo: CCW = left; CW = right

DEMO MODE 2 (Switch 1 = OFF; Switch 2 = OFF, Switch 3 = ON)

  • 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.

DEMO MODE 3 (Switch 1 = OFF; Switch 2 = ON, Switch 3 = ON)

  • Outputs camera data captured to terminal in CSV format, once every second.

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

  • If you want to hook up a scope, here are the pins of the camera interface from left to right: ground, 3.3V, SI, CLK, Analog_Out.
  • You want to hook up channel 1 between Analog_Out and Ground, channel 2 between SI and ground. SI can serve as your trigger.
  • Alternatively if you only want to use one channel, you can only hook up to Analog_Out and use triggering on the same channel.
  • 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_LEFT and MAX_STEER_RIGHT in Spices.cpp with these values to tell the program how your servo is hooked up.
  • Here's a video where values are output to LCD to make it easier: http://youtu.be/fp5gyfEMf50

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 function SpeedControl in Spices.cpp. There you can change which speed method you'd like to use. Currently is using value of 1, which gets maximum speed from TUNE_SPEED constant. Reduce to 0.4 or 0.5 when debugging car around the car (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

Spices/Spices.cpp@0:98e98e01a6ce, 2014-04-20 (annotated)

Committer:
redxeth
Date:
Sun Apr 20 03:33:03 2014 +0000
Revision:
0:98e98e01a6ce
Initial version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
redxeth 0:98e98e01a6ce 1 #include "mbed.h"
redxeth 0:98e98e01a6ce 2
redxeth 0:98e98e01a6ce 3 #include "Spices.h"
redxeth 0:98e98e01a6ce 4 #include "common.h"
redxeth 0:98e98e01a6ce 5 #include "TFC.h"
redxeth 0:98e98e01a6ce 6
redxeth 0:98e98e01a6ce 7 // camera params
redxeth 0:98e98e01a6ce 8 #define NUM_LINE_SCAN 128
redxeth 0:98e98e01a6ce 9 #define MAX_LINE_SCAN NUM_LINE_SCAN-1
redxeth 0:98e98e01a6ce 10 #define MIN_LINE_WIDTH 0
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 0:98e98e01a6ce 18 #define MAX_STEER_LEFT -0.51 // value determined by demo mode 1 measure (have to be adjusted with every servo horn attach)
redxeth 0:98e98e01a6ce 19 #define MAX_STEER_RIGHT 0.39 // 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
redxeth 0:98e98e01a6ce 140 float CurrentLinePosition; // Current position of track line (in pixels -- 0 to 127)
redxeth 0:98e98e01a6ce 141 float LastLinePosition; // Last position of track line (in pixels -- 0 to 127)
redxeth 0:98e98e01a6ce 142 float CurrentLinePosError = 0; // Current line position error (used for derivative calc)
redxeth 0:98e98e01a6ce 143 float AbsError;
redxeth 0:98e98e01a6ce 144 float LastLinePosError = 0; // Last line position error (used for derivative calc)
redxeth 0:98e98e01a6ce 145 float SumLinePosError = 0; // Sum of line position error (used for integral calc)
redxeth 0:98e98e01a6ce 146 float DerivError = 0; // Derivative of error
redxeth 0:98e98e01a6ce 147 float CurrentSteerSetting = (MAX_STEER_RIGHT + MAX_STEER_LEFT) / 2; // drive straight at first
redxeth 0:98e98e01a6ce 148 float CurrentLeftDriveSetting = 0; // Drive setting (left wheel)
redxeth 0:98e98e01a6ce 149 float CurrentRightDriveSetting = 0; // Drive setting (right wheel)
redxeth 0:98e98e01a6ce 150
redxeth 0:98e98e01a6ce 151 // Speed control vars
redxeth 0:98e98e01a6ce 152 float MaxSpeed; // maximum speed allowed
redxeth 0:98e98e01a6ce 153
redxeth 0:98e98e01a6ce 154 uint16_t startRaceTicker; // ticker at start of race1
redxeth 0:98e98e01a6ce 155
redxeth 0:98e98e01a6ce 156 // Custom Data Types
redxeth 0:98e98e01a6ce 157 typedef enum TrackStatusType {Unknown,
redxeth 0:98e98e01a6ce 158 LineFound,
redxeth 0:98e98e01a6ce 159 StartGateFound,
redxeth 0:98e98e01a6ce 160 LineJustLeft} TrackStatusType;
redxeth 0:98e98e01a6ce 161
redxeth 0:98e98e01a6ce 162 TrackStatusType CurrentTrackStatus; // current track status
redxeth 0:98e98e01a6ce 163 TrackStatusType LastTrackStatus; // last track status
redxeth 0:98e98e01a6ce 164
redxeth 0:98e98e01a6ce 165 /* typedef enum TrackType {NotSure,
redxeth 0:98e98e01a6ce 166 Straight,
redxeth 0:98e98e01a6ce 167 Curve,
redxeth 0:98e98e01a6ce 168 Wiggle,
redxeth 0:98e98e01a6ce 169 Bumps,
redxeth 0:98e98e01a6ce 170 StartGate,
redxeth 0:98e98e01a6ce 171 UpHill,
redxeth 0:98e98e01a6ce 172 DownHill} TrackType;
redxeth 0:98e98e01a6ce 173
redxeth 0:98e98e01a6ce 174 TrackType CurrentTrack; */
redxeth 0:98e98e01a6ce 175
redxeth 0:98e98e01a6ce 176
redxeth 0:98e98e01a6ce 177 struct LogData {
redxeth 0:98e98e01a6ce 178 float linepos;
redxeth 0:98e98e01a6ce 179 float steersetting;
redxeth 0:98e98e01a6ce 180 float leftdrivesetting;
redxeth 0:98e98e01a6ce 181 float rightdrivesetting;
redxeth 0:98e98e01a6ce 182 };
redxeth 0:98e98e01a6ce 183
redxeth 0:98e98e01a6ce 184 LogData frameLogs[NUM_LOG_FRAMES]; // array of log data to store
redxeth 0:98e98e01a6ce 185 int logDataIndex; // index for log data
redxeth 0:98e98e01a6ce 186
redxeth 0:98e98e01a6ce 187
redxeth 0:98e98e01a6ce 188 int StartGateFoundCount = 0; // how many times start gate has been found
redxeth 0:98e98e01a6ce 189 int UnknownCount = 0; // how many times nothing has been found (to help with kill switch implementation)
redxeth 0:98e98e01a6ce 190 bool go = false; // Car can go! Should be set to false to start.
redxeth 0:98e98e01a6ce 191
redxeth 0:98e98e01a6ce 192 // EXTRA CONTROL PARAMETERS
redxeth 0:98e98e01a6ce 193 bool debugFakeMode = false; // if true, ignores real camera and uses fake camera input instead; used for data processing debug
redxeth 0:98e98e01a6ce 194 int terminalOutput = 0; // set debug level for terminal output
redxeth 0:98e98e01a6ce 195 // 0 : no terminal output, race!
redxeth 0:98e98e01a6ce 196 // 1 : output just to measure frame rate
redxeth 0:98e98e01a6ce 197 // 2 : output for measuring time of operations
redxeth 0:98e98e01a6ce 198 // 3 : output with delay
redxeth 0:98e98e01a6ce 199 bool doLogData = false; // whether to capture log data to output later on
redxeth 0:98e98e01a6ce 200 bool killSwitch = false; // whether to enable Kill Switch (allow engine to stop after not finding track)
redxeth 0:98e98e01a6ce 201 bool startGateStop = false; // whether to stop or not depending on starting gate reading
redxeth 0:98e98e01a6ce 202 bool doRisky = false; // race style-- whether conservative or risky
redxeth 0:98e98e01a6ce 203
redxeth 0:98e98e01a6ce 204 // timer stuff
redxeth 0:98e98e01a6ce 205 Timer timer;
redxeth 0:98e98e01a6ce 206 int after_time, before_time, start_time, last_start_time;
redxeth 0:98e98e01a6ce 207 bool run_once = false;
redxeth 0:98e98e01a6ce 208
redxeth 0:98e98e01a6ce 209 void MasterControlProgram()
redxeth 0:98e98e01a6ce 210 {
redxeth 0:98e98e01a6ce 211
redxeth 0:98e98e01a6ce 212 // put here all things want to run only once after reset
redxeth 0:98e98e01a6ce 213 if (!run_once){
redxeth 0:98e98e01a6ce 214 if ((terminalOutput == 1) || (terminalOutput == 2)){
redxeth 0:98e98e01a6ce 215 timer.start();
redxeth 0:98e98e01a6ce 216 }
redxeth 0:98e98e01a6ce 217 run_once = true;
redxeth 0:98e98e01a6ce 218 }
redxeth 0:98e98e01a6ce 219
redxeth 0:98e98e01a6ce 220 // read DIP switches and Pots for data
redxeth 0:98e98e01a6ce 221 readSwitches();
redxeth 0:98e98e01a6ce 222
redxeth 0:98e98e01a6ce 223 // Every 4s (or Terminal Output is off, i.e. race mode!)
redxeth 0:98e98e01a6ce 224 // AND line scan image ready (or fake mode where image is always ready)
redxeth 0:98e98e01a6ce 225 // (ticker updates every 2ms) (Line scan image ready very 20mS?)
redxeth 0:98e98e01a6ce 226 if((TFC_Ticker[0]>2000 || (terminalOutput != 3)) && (TFC_LineScanImageReady>0 || debugFakeMode))
redxeth 0:98e98e01a6ce 227 {
redxeth 0:98e98e01a6ce 228
redxeth 0:98e98e01a6ce 229 // stuff that needs to be reset with each image frame
redxeth 0:98e98e01a6ce 230 if (terminalOutput == 1) {
redxeth 0:98e98e01a6ce 231 last_start_time = start_time;
redxeth 0:98e98e01a6ce 232 start_time = timer.read_us();
redxeth 0:98e98e01a6ce 233 TERMINAL_PRINTF("TIME:Between frames:%d:uSec\r\n", start_time - last_start_time);
redxeth 0:98e98e01a6ce 234 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 235 }
redxeth 0:98e98e01a6ce 236 TFC_Ticker[0] = 0;
redxeth 0:98e98e01a6ce 237 TFC_LineScanImageReady=0; // must reset to 0 after detecting non-zero
redxeth 0:98e98e01a6ce 238 MaxLightIntensity = 0; // reset
redxeth 0:98e98e01a6ce 239 MinLightIntensity = (1 << 12); // reset
redxeth 0:98e98e01a6ce 240
redxeth 0:98e98e01a6ce 241 // grab camera frame
redxeth 0:98e98e01a6ce 242 grabCameraFrame();
redxeth 0:98e98e01a6ce 243
redxeth 0:98e98e01a6ce 244 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 245 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 246 TERMINAL_PRINTF("TIME:TO AFTER grabCameraFrame:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 247 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 248 }
redxeth 0:98e98e01a6ce 249
redxeth 0:98e98e01a6ce 250 // calcalate derivative of linescandata, filter starttime data
redxeth 0:98e98e01a6ce 251 derivativeLineScan(&GrabLineScanImage0[0], &DerivLineScanImage0[0]);
redxeth 0:98e98e01a6ce 252
redxeth 0:98e98e01a6ce 253 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 254 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 255 TERMINAL_PRINTF("TIME:TO AFTER derivativeLineScan:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 256 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 257 }
redxeth 0:98e98e01a6ce 258
redxeth 0:98e98e01a6ce 259 // adjust deriv threshold based on max lighting value
redxeth 0:98e98e01a6ce 260 // has to be called before find edges
redxeth 0:98e98e01a6ce 261 adjustLights();
redxeth 0:98e98e01a6ce 262
redxeth 0:98e98e01a6ce 263 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 264 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 265 TERMINAL_PRINTF("TIME:TO AFTER adjustLights:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 266 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 267 }
redxeth 0:98e98e01a6ce 268
redxeth 0:98e98e01a6ce 269 //find edges from derivative data
redxeth 0:98e98e01a6ce 270 findEdges_v2(&DerivLineScanImage0[0]);
redxeth 0:98e98e01a6ce 271
redxeth 0:98e98e01a6ce 272 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 273 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 274 TERMINAL_PRINTF("TIME:TO AFTER findEdges_v2:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 275 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 276 }
redxeth 0:98e98e01a6ce 277
redxeth 0:98e98e01a6ce 278 // turn on terminal output if line not found -- FOR DEBUG
redxeth 0:98e98e01a6ce 279 //if (CurrentTrackStatus == Unknown)
redxeth 0:98e98e01a6ce 280 // terminalOutput = 1;
redxeth 0:98e98e01a6ce 281
redxeth 0:98e98e01a6ce 282 //review edge data and set position or starting flag appropriately
redxeth 0:98e98e01a6ce 283 reviewEdges();
redxeth 0:98e98e01a6ce 284
redxeth 0:98e98e01a6ce 285 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 286 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 287 TERMINAL_PRINTF("TIME:TO AFTER reviewEdges:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 288 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 289 }
redxeth 0:98e98e01a6ce 290
redxeth 0:98e98e01a6ce 291 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 292 // print data to Terminal for camera 0
redxeth 0:98e98e01a6ce 293 printLineScanData(&GrabLineScanImage0[0]);
redxeth 0:98e98e01a6ce 294
redxeth 0:98e98e01a6ce 295 // print deriviative of linescandata, filter starttime data
redxeth 0:98e98e01a6ce 296 printDerivLineScanData(&DerivLineScanImage0[0]);
redxeth 0:98e98e01a6ce 297
redxeth 0:98e98e01a6ce 298 printAdjustLightsData();
redxeth 0:98e98e01a6ce 299
redxeth 0:98e98e01a6ce 300 printEdgesFound();
redxeth 0:98e98e01a6ce 301
redxeth 0:98e98e01a6ce 302 }
redxeth 0:98e98e01a6ce 303
redxeth 0:98e98e01a6ce 304 // ** Track Status available at this point **
redxeth 0:98e98e01a6ce 305
redxeth 0:98e98e01a6ce 306
redxeth 0:98e98e01a6ce 307 // test out accelerometer
redxeth 0:98e98e01a6ce 308 // accelTest();
redxeth 0:98e98e01a6ce 309
redxeth 0:98e98e01a6ce 310 // Update things based on latest track status
redxeth 0:98e98e01a6ce 311 // e.g. change steering setting, stop car, ...
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 }
redxeth 0:98e98e01a6ce 319
redxeth 0:98e98e01a6ce 320
redxeth 0:98e98e01a6ce 321 //give LED feedback as to track status
redxeth 0:98e98e01a6ce 322 feedbackLights();
redxeth 0:98e98e01a6ce 323
redxeth 0:98e98e01a6ce 324 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 325 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 326 TERMINAL_PRINTF("TIME:TO AFTER feedbackLights:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 327 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 328 }
redxeth 0:98e98e01a6ce 329
redxeth 0:98e98e01a6ce 330 // control max power (speed)
redxeth 0:98e98e01a6ce 331 SpeedControl();
redxeth 0:98e98e01a6ce 332
redxeth 0:98e98e01a6ce 333 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 334 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 335 TERMINAL_PRINTF("TIME:TO AFTER SpeedControl:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 336 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 337 }
redxeth 0:98e98e01a6ce 338
redxeth 0:98e98e01a6ce 339 // Drive!!
redxeth 0:98e98e01a6ce 340 Drive();
redxeth 0:98e98e01a6ce 341
redxeth 0:98e98e01a6ce 342 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 343 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 344 TERMINAL_PRINTF("TIME:TO AFTER Drive:%d:uSec\r\n", after_time - before_time);
redxeth 0:98e98e01a6ce 345 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 346 }
redxeth 0:98e98e01a6ce 347
redxeth 0:98e98e01a6ce 348 // wait_ms(1);
redxeth 0:98e98e01a6ce 349
redxeth 0:98e98e01a6ce 350 // Capture Log data while driving
redxeth 0:98e98e01a6ce 351 if (go && doLogData) {
redxeth 0:98e98e01a6ce 352 captureData();
redxeth 0:98e98e01a6ce 353 }
redxeth 0:98e98e01a6ce 354
redxeth 0:98e98e01a6ce 355 // Dump Log data to Terminal while stopped and holding B button
redxeth 0:98e98e01a6ce 356 if (!go && doLogData && TFC_PUSH_BUTTON_1_PRESSED) {
redxeth 0:98e98e01a6ce 357 dumpData();
redxeth 0:98e98e01a6ce 358 }
redxeth 0:98e98e01a6ce 359
redxeth 0:98e98e01a6ce 360 if (terminalOutput == 2) {
redxeth 0:98e98e01a6ce 361 after_time = timer.read_us();
redxeth 0:98e98e01a6ce 362 TERMINAL_PRINTF("TIME: ENTIRE FRAME (include prints):%d:uSec\r\n", after_time - start_time);
redxeth 0:98e98e01a6ce 363 before_time = timer.read_us();
redxeth 0:98e98e01a6ce 364 }
redxeth 0:98e98e01a6ce 365
redxeth 0:98e98e01a6ce 366 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 367 TERMINAL_PRINTF("\r\n**************************END********************************\r\n");
redxeth 0:98e98e01a6ce 368 }
redxeth 0:98e98e01a6ce 369
redxeth 0:98e98e01a6ce 370 }
redxeth 0:98e98e01a6ce 371 }
redxeth 0:98e98e01a6ce 372
redxeth 0:98e98e01a6ce 373 void dumpData()
redxeth 0:98e98e01a6ce 374 {
redxeth 0:98e98e01a6ce 375 TERMINAL_PRINTF("INDEX,LINEPOS,STEERSETTING,LEFTDRIVESETTING,RIGHTDRIVESETTING\r\n");
redxeth 0:98e98e01a6ce 376 for(logDataIndex=0;logDataIndex<NUM_LOG_FRAMES;logDataIndex++) {
redxeth 0:98e98e01a6ce 377 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 378 }
redxeth 0:98e98e01a6ce 379 }
redxeth 0:98e98e01a6ce 380
redxeth 0:98e98e01a6ce 381 void captureData()
redxeth 0:98e98e01a6ce 382 {
redxeth 0:98e98e01a6ce 383 frameLogs[logDataIndex].linepos = CurrentLinePosition;
redxeth 0:98e98e01a6ce 384 frameLogs[logDataIndex].steersetting = CurrentSteerSetting;
redxeth 0:98e98e01a6ce 385 frameLogs[logDataIndex].leftdrivesetting = CurrentLeftDriveSetting;
redxeth 0:98e98e01a6ce 386 frameLogs[logDataIndex].rightdrivesetting = CurrentRightDriveSetting;
redxeth 0:98e98e01a6ce 387
redxeth 0:98e98e01a6ce 388 // increment index
redxeth 0:98e98e01a6ce 389 logDataIndex++;
redxeth 0:98e98e01a6ce 390 if (logDataIndex > NUM_LOG_FRAMES)
redxeth 0:98e98e01a6ce 391 logDataIndex = 0;
redxeth 0:98e98e01a6ce 392 }
redxeth 0:98e98e01a6ce 393
redxeth 0:98e98e01a6ce 394 void readSwitches()
redxeth 0:98e98e01a6ce 395 {
redxeth 0:98e98e01a6ce 396
redxeth 0:98e98e01a6ce 397 // ********* GATHER DIP SWITCH INPUTS *********
redxeth 0:98e98e01a6ce 398 if(TFC_GetDIP_Switch()&0x02) // SWITCH 2
redxeth 0:98e98e01a6ce 399 doLogData = true; // Log data to array
redxeth 0:98e98e01a6ce 400 else
redxeth 0:98e98e01a6ce 401 doLogData = false; // normal operation
redxeth 0:98e98e01a6ce 402
redxeth 0:98e98e01a6ce 403 if(TFC_GetDIP_Switch()&0x04) // SWITCH 3
redxeth 0:98e98e01a6ce 404 doRisky = true;
redxeth 0:98e98e01a6ce 405 else
redxeth 0:98e98e01a6ce 406 doRisky = false;
redxeth 0:98e98e01a6ce 407
redxeth 0:98e98e01a6ce 408 if(TFC_GetDIP_Switch()&0x08) // SWITCH 4 control start stop gate
redxeth 0:98e98e01a6ce 409 startGateStop = true;
redxeth 0:98e98e01a6ce 410 else
redxeth 0:98e98e01a6ce 411 startGateStop = false;
redxeth 0:98e98e01a6ce 412
redxeth 0:98e98e01a6ce 413
redxeth 0:98e98e01a6ce 414 }
redxeth 0:98e98e01a6ce 415
redxeth 0:98e98e01a6ce 416 void grabCameraFrame()
redxeth 0:98e98e01a6ce 417 {
redxeth 0:98e98e01a6ce 418 uint32_t i = 0;
redxeth 0:98e98e01a6ce 419 uint8_t fake_type = 4; // type of fake data if used
redxeth 0:98e98e01a6ce 420
redxeth 0:98e98e01a6ce 421 for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data (128) from Camera 0
redxeth 0:98e98e01a6ce 422 {
redxeth 0:98e98e01a6ce 423
redxeth 0:98e98e01a6ce 424 if (debugFakeMode) { // use fake camera data
redxeth 0:98e98e01a6ce 425 switch (fake_type) {
redxeth 0:98e98e01a6ce 426 case 0: // ideal track -- line in center
redxeth 0:98e98e01a6ce 427 if (i<57 || i > 70)
redxeth 0:98e98e01a6ce 428 GrabLineScanImage0[i] = 0xFFF; // no line
redxeth 0:98e98e01a6ce 429 else
redxeth 0:98e98e01a6ce 430 GrabLineScanImage0[i] = 0x4B0; // line
redxeth 0:98e98e01a6ce 431 break;
redxeth 0:98e98e01a6ce 432 case 1: // ideal track -- line to the left
redxeth 0:98e98e01a6ce 433 if (i<27 || i > 40)
redxeth 0:98e98e01a6ce 434 GrabLineScanImage0[i] = 0xFFF; // no line
redxeth 0:98e98e01a6ce 435 else
redxeth 0:98e98e01a6ce 436 GrabLineScanImage0[i] = 0x4B0; // line
redxeth 0:98e98e01a6ce 437 break;
redxeth 0:98e98e01a6ce 438 case 2: // ideal track -- line to the right
redxeth 0:98e98e01a6ce 439 if (i<87 || i > 100)
redxeth 0:98e98e01a6ce 440 GrabLineScanImage0[i] = 0xFFF; // no line
redxeth 0:98e98e01a6ce 441 else
redxeth 0:98e98e01a6ce 442 GrabLineScanImage0[i] = 0x4B0; // line
redxeth 0:98e98e01a6ce 443 break;
redxeth 0:98e98e01a6ce 444 case 3: // ideal track -- starting gate!
redxeth 0:98e98e01a6ce 445 // TBD
redxeth 0:98e98e01a6ce 446 break;
redxeth 0:98e98e01a6ce 447 case 4: // less than ideal track -- debug multi-edge issue!
redxeth 0:98e98e01a6ce 448 if (i<54)
redxeth 0:98e98e01a6ce 449 GrabLineScanImage0[i] = 4000; // no line
redxeth 0:98e98e01a6ce 450 if (i == 54)
redxeth 0:98e98e01a6ce 451 GrabLineScanImage0[i] = 3370; // neg edge
redxeth 0:98e98e01a6ce 452 if (i == 55)
redxeth 0:98e98e01a6ce 453 GrabLineScanImage0[i] = 3309; // neg edge
redxeth 0:98e98e01a6ce 454 if (i == 56)
redxeth 0:98e98e01a6ce 455 GrabLineScanImage0[i] = 2016; // neg edge
redxeth 0:98e98e01a6ce 456 if (i == 57)
redxeth 0:98e98e01a6ce 457 GrabLineScanImage0[i] = 711; // neg edge
redxeth 0:98e98e01a6ce 458 if (i == 58)
redxeth 0:98e98e01a6ce 459 GrabLineScanImage0[i] = 696; // neg edge
redxeth 0:98e98e01a6ce 460 if ((i>58) && (i<69))
redxeth 0:98e98e01a6ce 461 GrabLineScanImage0[i] = 500; // line
redxeth 0:98e98e01a6ce 462 if (i == 69)
redxeth 0:98e98e01a6ce 463 GrabLineScanImage0[i] = 1800; // pos edge
redxeth 0:98e98e01a6ce 464 if (i > 69)
redxeth 0:98e98e01a6ce 465 GrabLineScanImage0[i] = 4000; // no line
redxeth 0:98e98e01a6ce 466 default:
redxeth 0:98e98e01a6ce 467 break;
redxeth 0:98e98e01a6ce 468 }
redxeth 0:98e98e01a6ce 469
redxeth 0:98e98e01a6ce 470 } else { // use real camera data
redxeth 0:98e98e01a6ce 471 GrabLineScanImage0[i] = TFC_LineScanImage0[i];
redxeth 0:98e98e01a6ce 472 }
redxeth 0:98e98e01a6ce 473 }
redxeth 0:98e98e01a6ce 474
redxeth 0:98e98e01a6ce 475
redxeth 0:98e98e01a6ce 476 }
redxeth 0:98e98e01a6ce 477
redxeth 0:98e98e01a6ce 478 void printLineScanData(uint16_t* LineScanData)
redxeth 0:98e98e01a6ce 479 {
redxeth 0:98e98e01a6ce 480 uint32_t i = 0;
redxeth 0:98e98e01a6ce 481 float Val;
redxeth 0:98e98e01a6ce 482
redxeth 0:98e98e01a6ce 483 TERMINAL_PRINTF("LINE SCAN DATA:,");
redxeth 0:98e98e01a6ce 484
redxeth 0:98e98e01a6ce 485 for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data (128) from Camera 0
redxeth 0:98e98e01a6ce 486 {
redxeth 0:98e98e01a6ce 487 if (1 == 1) { // use float to print
redxeth 0:98e98e01a6ce 488 Val = (float) LineScanData[i];
redxeth 0:98e98e01a6ce 489 TERMINAL_PRINTF("%9.3f",Val);
redxeth 0:98e98e01a6ce 490 if(i==MAX_LINE_SCAN) // when last data reached put in line return
redxeth 0:98e98e01a6ce 491 TERMINAL_PRINTF("\r\n");
redxeth 0:98e98e01a6ce 492 else
redxeth 0:98e98e01a6ce 493 TERMINAL_PRINTF(",");
redxeth 0:98e98e01a6ce 494 } else {
redxeth 0:98e98e01a6ce 495 TERMINAL_PRINTF("0x%X",LineScanData[i]);
redxeth 0:98e98e01a6ce 496 if(i==MAX_LINE_SCAN) // when last data reached put in line return
redxeth 0:98e98e01a6ce 497 TERMINAL_PRINTF("\r\n",LineScanData[i]);
redxeth 0:98e98e01a6ce 498 else
redxeth 0:98e98e01a6ce 499 TERMINAL_PRINTF(",",LineScanData[i]);
redxeth 0:98e98e01a6ce 500 }
redxeth 0:98e98e01a6ce 501 }
redxeth 0:98e98e01a6ce 502
redxeth 0:98e98e01a6ce 503 }
redxeth 0:98e98e01a6ce 504
redxeth 0:98e98e01a6ce 505 void printDerivLineScanData(float* derivLineScanData)
redxeth 0:98e98e01a6ce 506 {
redxeth 0:98e98e01a6ce 507 uint32_t i, minCnt = 0, maxCnt = 0;
redxeth 0:98e98e01a6ce 508
redxeth 0:98e98e01a6ce 509 minCnt = FILTER_ENDS;
redxeth 0:98e98e01a6ce 510 maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
redxeth 0:98e98e01a6ce 511
redxeth 0:98e98e01a6ce 512 TERMINAL_PRINTF("DERIVATIVE DATA:,");
redxeth 0:98e98e01a6ce 513
redxeth 0:98e98e01a6ce 514 for(i=minCnt;i<maxCnt;i++) // print one line worth of data (128) from Camera 0
redxeth 0:98e98e01a6ce 515 {
redxeth 0:98e98e01a6ce 516 TERMINAL_PRINTF("%9.3f",derivLineScanData[i]);
redxeth 0:98e98e01a6ce 517 if(i==maxCnt-1) // when last data reached put in line return
redxeth 0:98e98e01a6ce 518 TERMINAL_PRINTF("\r\n",derivLineScanData[i]);
redxeth 0:98e98e01a6ce 519 else
redxeth 0:98e98e01a6ce 520 TERMINAL_PRINTF(", ",derivLineScanData[i]);
redxeth 0:98e98e01a6ce 521 }
redxeth 0:98e98e01a6ce 522
redxeth 0:98e98e01a6ce 523 }
redxeth 0:98e98e01a6ce 524
redxeth 0:98e98e01a6ce 525 void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut)
redxeth 0:98e98e01a6ce 526 {
redxeth 0:98e98e01a6ce 527
redxeth 0:98e98e01a6ce 528 uint32_t i, minCnt = 0, maxCnt = 0;
redxeth 0:98e98e01a6ce 529 float DerVal, upperDerVal, lowerDerVal = 0;
redxeth 0:98e98e01a6ce 530
redxeth 0:98e98e01a6ce 531 maxDerVal = 0;
redxeth 0:98e98e01a6ce 532 minDerVal = (float) (1 << 12);
redxeth 0:98e98e01a6ce 533 aveDerVal = 0;
redxeth 0:98e98e01a6ce 534
redxeth 0:98e98e01a6ce 535 minCnt = FILTER_ENDS;
redxeth 0:98e98e01a6ce 536 maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
redxeth 0:98e98e01a6ce 537
redxeth 0:98e98e01a6ce 538 // TERMINAL_PRINTF("i, upperDerVal, lowerDerVal, DerVal\r\n");
redxeth 0:98e98e01a6ce 539
redxeth 0:98e98e01a6ce 540 for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0
redxeth 0:98e98e01a6ce 541 {
redxeth 0:98e98e01a6ce 542
redxeth 0:98e98e01a6ce 543 // store max light intensity value
redxeth 0:98e98e01a6ce 544 if (LineScanDataIn[i] > MaxLightIntensity)
redxeth 0:98e98e01a6ce 545 MaxLightIntensity = LineScanDataIn[i];
redxeth 0:98e98e01a6ce 546
redxeth 0:98e98e01a6ce 547 // store min light intensity value
redxeth 0:98e98e01a6ce 548 if (LineScanDataIn[i] < MinLightIntensity)
redxeth 0:98e98e01a6ce 549 MinLightIntensity = LineScanDataIn[i];
redxeth 0:98e98e01a6ce 550
redxeth 0:98e98e01a6ce 551
redxeth 0:98e98e01a6ce 552 // Central Derivative
redxeth 0:98e98e01a6ce 553 if (i==minCnt) { // start point
redxeth 0:98e98e01a6ce 554 upperDerVal = (float)(LineScanDataIn[i+1]);
redxeth 0:98e98e01a6ce 555 lowerDerVal = (float)(LineScanDataIn[i]); // make same as start point
redxeth 0:98e98e01a6ce 556 } else if (i==maxCnt - 1){ // end point
redxeth 0:98e98e01a6ce 557 upperDerVal = (float)(LineScanDataIn[i]); // make same as end point
redxeth 0:98e98e01a6ce 558 lowerDerVal = (float)(LineScanDataIn[i-1]);
redxeth 0:98e98e01a6ce 559 } else { // any other point
redxeth 0:98e98e01a6ce 560 upperDerVal = (float)(LineScanDataIn[i+1]);
redxeth 0:98e98e01a6ce 561 lowerDerVal = (float)(LineScanDataIn[i-1]);
redxeth 0:98e98e01a6ce 562 }
redxeth 0:98e98e01a6ce 563 DerVal = (upperDerVal - lowerDerVal) / 2;
redxeth 0:98e98e01a6ce 564 // TERMINAL_PRINTF("%d,%9.3f,%9.3f,%9.3f\r\n", i, upperDerVal, lowerDerVal, DerVal);
redxeth 0:98e98e01a6ce 565
redxeth 0:98e98e01a6ce 566 if (DerVal > maxDerVal) {
redxeth 0:98e98e01a6ce 567 maxDerVal = DerVal;
redxeth 0:98e98e01a6ce 568 }
redxeth 0:98e98e01a6ce 569 if (DerVal < minDerVal) {
redxeth 0:98e98e01a6ce 570 minDerVal = DerVal;
redxeth 0:98e98e01a6ce 571 }
redxeth 0:98e98e01a6ce 572 aveDerVal = aveDerVal + DerVal; //get sum
redxeth 0:98e98e01a6ce 573 DerivLineScanDataOut[i] = DerVal;
redxeth 0:98e98e01a6ce 574 }
redxeth 0:98e98e01a6ce 575 aveDerVal = (float) aveDerVal / (maxCnt - minCnt);
redxeth 0:98e98e01a6ce 576 }
redxeth 0:98e98e01a6ce 577
redxeth 0:98e98e01a6ce 578 //Not reliable for finding edges!
redxeth 0:98e98e01a6ce 579 void findEdges(float* derivLineScanData)
redxeth 0:98e98e01a6ce 580 {
redxeth 0:98e98e01a6ce 581 // search for edges in deriviative data using a threshold
redxeth 0:98e98e01a6ce 582 // need to store in a hash if that's possible...
redxeth 0:98e98e01a6ce 583 // combine edges that are a pixel apart
redxeth 0:98e98e01a6ce 584
redxeth 0:98e98e01a6ce 585 int i, minCnt = 0, maxCnt = 0;
redxeth 0:98e98e01a6ce 586 int multiNegEdgeCnt = 1, multiNegEdgeSum = 0;
redxeth 0:98e98e01a6ce 587 int multiPosEdgeCnt = 1, multiPosEdgeSum = 0;
redxeth 0:98e98e01a6ce 588
redxeth 0:98e98e01a6ce 589 minCnt = FILTER_ENDS;
redxeth 0:98e98e01a6ce 590 maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
redxeth 0:98e98e01a6ce 591
redxeth 0:98e98e01a6ce 592 numNegEdges = 0;
redxeth 0:98e98e01a6ce 593 numPosEdges = 0;
redxeth 0:98e98e01a6ce 594 for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0
redxeth 0:98e98e01a6ce 595 {
redxeth 0:98e98e01a6ce 596 if (derivLineScanData[i] <= NegDerivThreshold) // NEGATIVE EDGE FOUND!
redxeth 0:98e98e01a6ce 597 {
redxeth 0:98e98e01a6ce 598 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 599 TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
redxeth 0:98e98e01a6ce 600 }
redxeth 0:98e98e01a6ce 601
redxeth 0:98e98e01a6ce 602 if ((numNegEdges > 0) && (NegEdges[numNegEdges - 1] + 1 == i )) // if no multi edges
redxeth 0:98e98e01a6ce 603 { // edge actually across multiple pixels
redxeth 0:98e98e01a6ce 604 multiNegEdgeCnt++;
redxeth 0:98e98e01a6ce 605 multiNegEdgeSum = multiNegEdgeSum + i;
redxeth 0:98e98e01a6ce 606 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 607 TERMINAL_PRINTF("MULTIEDGE FOUND! MultiNegEdgeCnt: %d; MultiNegEdgeSum: %d\r\n", multiNegEdgeCnt, multiNegEdgeSum);
redxeth 0:98e98e01a6ce 608 }
redxeth 0:98e98e01a6ce 609 } else { // not a multi-pixel edge known at this time, store negative edge index value
redxeth 0:98e98e01a6ce 610 numNegEdges++;
redxeth 0:98e98e01a6ce 611 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 612 TERMINAL_PRINTF("NEG EDGE STORED WITH INDEX %d. NUM NEG EDGES = %d\r\n", i, numNegEdges);
redxeth 0:98e98e01a6ce 613 }
redxeth 0:98e98e01a6ce 614 NegEdges[numNegEdges - 1] = (float) i;
redxeth 0:98e98e01a6ce 615 multiNegEdgeSum = i;
redxeth 0:98e98e01a6ce 616 }
redxeth 0:98e98e01a6ce 617
redxeth 0:98e98e01a6ce 618
redxeth 0:98e98e01a6ce 619 } else if (derivLineScanData[i] > PosDerivThreshold) { // POSITIVE EDGE FOUND!
redxeth 0:98e98e01a6ce 620
redxeth 0:98e98e01a6ce 621 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 622 TERMINAL_PRINTF("POS EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
redxeth 0:98e98e01a6ce 623 }
redxeth 0:98e98e01a6ce 624
redxeth 0:98e98e01a6ce 625 if ((numPosEdges > 0) && (PosEdges[numPosEdges - 1] + 1 == i ))
redxeth 0:98e98e01a6ce 626 { // edge actually across multiple pixels
redxeth 0:98e98e01a6ce 627 multiPosEdgeCnt++;
redxeth 0:98e98e01a6ce 628 multiPosEdgeSum = multiPosEdgeSum + i;
redxeth 0:98e98e01a6ce 629 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 630 TERMINAL_PRINTF("MULTIEDGE FOUND! MultiPosEdgeCnt: %d; MultiPosEdgeSum: %d\r\n", multiPosEdgeCnt, multiPosEdgeSum);
redxeth 0:98e98e01a6ce 631 }
redxeth 0:98e98e01a6ce 632 } else { // not a multi-pixel edge known at this time, store Posative edge index value
redxeth 0:98e98e01a6ce 633 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 634 TERMINAL_PRINTF("POS EDGE STORED WITH INDEX %d. NUM POS EDGES = %d\r\n", i, numPosEdges);
redxeth 0:98e98e01a6ce 635 }
redxeth 0:98e98e01a6ce 636 numPosEdges++;
redxeth 0:98e98e01a6ce 637 PosEdges[numPosEdges - 1] = (float) i;
redxeth 0:98e98e01a6ce 638 multiPosEdgeSum = i;
redxeth 0:98e98e01a6ce 639 }
redxeth 0:98e98e01a6ce 640
redxeth 0:98e98e01a6ce 641 } else { // NO EDGE FOUND
redxeth 0:98e98e01a6ce 642 // combine multi-edges if there are any
redxeth 0:98e98e01a6ce 643 if (multiNegEdgeCnt > 1)
redxeth 0:98e98e01a6ce 644 {
redxeth 0:98e98e01a6ce 645 NegEdges[numNegEdges - 1] = (float) multiNegEdgeSum / multiNegEdgeCnt;
redxeth 0:98e98e01a6ce 646 multiNegEdgeCnt = 1; multiNegEdgeSum = 0;
redxeth 0:98e98e01a6ce 647 }
redxeth 0:98e98e01a6ce 648 if (multiPosEdgeCnt > 1)
redxeth 0:98e98e01a6ce 649 {
redxeth 0:98e98e01a6ce 650 PosEdges[numPosEdges - 1] = (float) multiPosEdgeSum / multiPosEdgeCnt;
redxeth 0:98e98e01a6ce 651 multiPosEdgeCnt = 1; multiPosEdgeSum = 0;
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
redxeth 0:98e98e01a6ce 660 void findEdges_v2(float* derivLineScanData)
redxeth 0:98e98e01a6ce 661 {
redxeth 0:98e98e01a6ce 662 // search for edges in deriviative data using a threshold
redxeth 0:98e98e01a6ce 663 // need to store in a hash if that's possible...
redxeth 0:98e98e01a6ce 664 // combine edges that are a pixel apart
redxeth 0:98e98e01a6ce 665
redxeth 0:98e98e01a6ce 666 int i;
redxeth 0:98e98e01a6ce 667
redxeth 0:98e98e01a6ce 668 int NegEdgeBufCnt = 0, NegEdgeBufSum = 0; // serves as buffer to store neg edges found next to each other
redxeth 0:98e98e01a6ce 669 int PosEdgeBufCnt = 0, PosEdgeBufSum = 0; // serves as buffer to store pos edges found next to each other
redxeth 0:98e98e01a6ce 670
redxeth 0:98e98e01a6ce 671 int minCnt = FILTER_ENDS;
redxeth 0:98e98e01a6ce 672 int maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
redxeth 0:98e98e01a6ce 673
redxeth 0:98e98e01a6ce 674
redxeth 0:98e98e01a6ce 675
redxeth 0:98e98e01a6ce 676 numNegEdges = 0; // count of neg edges found thus far
redxeth 0:98e98e01a6ce 677 numPosEdges = 0; // count of pos edges found thus far
redxeth 0:98e98e01a6ce 678 for(i=minCnt;i<maxCnt;i++) // print one line worth of data from Camera 0
redxeth 0:98e98e01a6ce 679 {
redxeth 0:98e98e01a6ce 680
redxeth 0:98e98e01a6ce 681 if (derivLineScanData[i] <= NegDerivThreshold) // NEGATIVE EDGE FOUND!
redxeth 0:98e98e01a6ce 682 {
redxeth 0:98e98e01a6ce 683
redxeth 0:98e98e01a6ce 684 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 685 TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
redxeth 0:98e98e01a6ce 686 }
redxeth 0:98e98e01a6ce 687
redxeth 0:98e98e01a6ce 688 NegEdgeBufCnt++; // add value to neg edge buffer
redxeth 0:98e98e01a6ce 689 NegEdgeBufSum = NegEdgeBufSum + i;
redxeth 0:98e98e01a6ce 690
redxeth 0:98e98e01a6ce 691 } else if (derivLineScanData[i] > PosDerivThreshold) { // POSITIVE EDGE FOUND!
redxeth 0:98e98e01a6ce 692
redxeth 0:98e98e01a6ce 693 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 694 TERMINAL_PRINTF("POS EDGE FOUND AT INDEX %d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
redxeth 0:98e98e01a6ce 695 }
redxeth 0:98e98e01a6ce 696
redxeth 0:98e98e01a6ce 697 PosEdgeBufCnt++; // add value to pos edge buffer
redxeth 0:98e98e01a6ce 698 PosEdgeBufSum = PosEdgeBufSum + i;
redxeth 0:98e98e01a6ce 699
redxeth 0:98e98e01a6ce 700 } else { // NO EDGE FOUND
redxeth 0:98e98e01a6ce 701
redxeth 0:98e98e01a6ce 702 // POP EDGE BUFFERS IF NON-EMPTY AND STORE TO EDGE "STACK" (i.e. edges found)
redxeth 0:98e98e01a6ce 703
redxeth 0:98e98e01a6ce 704 if (NegEdgeBufCnt > 0) {
redxeth 0:98e98e01a6ce 705 // store edge value
redxeth 0:98e98e01a6ce 706 numNegEdges++;
redxeth 0:98e98e01a6ce 707 NegEdges[numNegEdges - 1] = (float) NegEdgeBufSum / NegEdgeBufCnt;
redxeth 0:98e98e01a6ce 708
redxeth 0:98e98e01a6ce 709 // clear edge buffer
redxeth 0:98e98e01a6ce 710 NegEdgeBufSum = 0; NegEdgeBufCnt = 0;
redxeth 0:98e98e01a6ce 711 }
redxeth 0:98e98e01a6ce 712
redxeth 0:98e98e01a6ce 713 if (PosEdgeBufCnt > 0) {
redxeth 0:98e98e01a6ce 714 // store edge value
redxeth 0:98e98e01a6ce 715 numPosEdges++;
redxeth 0:98e98e01a6ce 716 PosEdges[numPosEdges - 1] = (float) PosEdgeBufSum / PosEdgeBufCnt;
redxeth 0:98e98e01a6ce 717
redxeth 0:98e98e01a6ce 718 // clear edge buffer
redxeth 0:98e98e01a6ce 719 PosEdgeBufSum = 0; PosEdgeBufCnt = 0;
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
redxeth 0:98e98e01a6ce 728 void printEdgesFound()
redxeth 0:98e98e01a6ce 729 {
redxeth 0:98e98e01a6ce 730 int i;
redxeth 0:98e98e01a6ce 731
redxeth 0:98e98e01a6ce 732 // Check that neg edges captured ok
redxeth 0:98e98e01a6ce 733 TERMINAL_PRINTF("NEGATIVE EDGES FOUND:,");
redxeth 0:98e98e01a6ce 734 for(i=0;i<=numNegEdges-1;i++)
redxeth 0:98e98e01a6ce 735 {
redxeth 0:98e98e01a6ce 736 TERMINAL_PRINTF("%9.3f",NegEdges[i]);
redxeth 0:98e98e01a6ce 737 if(i==numNegEdges-1) // when last data reached put in line return
redxeth 0:98e98e01a6ce 738 TERMINAL_PRINTF("\r\n");
redxeth 0:98e98e01a6ce 739 else
redxeth 0:98e98e01a6ce 740 TERMINAL_PRINTF(", ");
redxeth 0:98e98e01a6ce 741 }
redxeth 0:98e98e01a6ce 742
redxeth 0:98e98e01a6ce 743
redxeth 0:98e98e01a6ce 744 // Check that pos edges captured ok
redxeth 0:98e98e01a6ce 745 TERMINAL_PRINTF("POSITIVE EDGES FOUND:,");
redxeth 0:98e98e01a6ce 746 for(i=0;i<=numPosEdges-1;i++)
redxeth 0:98e98e01a6ce 747 {
redxeth 0:98e98e01a6ce 748 TERMINAL_PRINTF("%9.3f",PosEdges[i]);
redxeth 0:98e98e01a6ce 749 if(i==numPosEdges-1) // when last data reached put in line return
redxeth 0:98e98e01a6ce 750 TERMINAL_PRINTF("\r\n");
redxeth 0:98e98e01a6ce 751 else
redxeth 0:98e98e01a6ce 752 TERMINAL_PRINTF(", ");
redxeth 0:98e98e01a6ce 753 }
redxeth 0:98e98e01a6ce 754
redxeth 0:98e98e01a6ce 755 }
redxeth 0:98e98e01a6ce 756
redxeth 0:98e98e01a6ce 757 void reviewEdges()
redxeth 0:98e98e01a6ce 758 {
redxeth 0:98e98e01a6ce 759 LastTrackStatus = CurrentTrackStatus;
redxeth 0:98e98e01a6ce 760
redxeth 0:98e98e01a6ce 761 if ((numPosEdges == 1) && (numNegEdges == 1)) // only one negative and positive edge found (LINE)
redxeth 0:98e98e01a6ce 762 {
redxeth 0:98e98e01a6ce 763 if (((PosEdges[0] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[0] - NegEdges[0]) <= MAX_LINE_WIDTH)) // has proper expected width
redxeth 0:98e98e01a6ce 764 {
redxeth 0:98e98e01a6ce 765 CurrentTrackStatus = LineFound; // report line found!
redxeth 0:98e98e01a6ce 766 UnknownCount = 0; // reset unknown status count
redxeth 0:98e98e01a6ce 767 LastLinePosition = CurrentLinePosition;
redxeth 0:98e98e01a6ce 768 CurrentLinePosition = (PosEdges[0]+NegEdges[0]) / 2; // update line position
redxeth 0:98e98e01a6ce 769 }
redxeth 0:98e98e01a6ce 770 } else if ((numPosEdges == 1) && (numNegEdges == 0)) { // 1 pos edge found (POSSIBLE LINE)
redxeth 0:98e98e01a6ce 771 if ((PosEdges[0] <= MAX_LINE_WIDTH) && (LastLinePosError < 0)) // pos edge is within line width of edge of camera (LEFT)
redxeth 0:98e98e01a6ce 772 {
redxeth 0:98e98e01a6ce 773 CurrentTrackStatus = LineFound; // report line found!
redxeth 0:98e98e01a6ce 774 UnknownCount = 0; // reset unknown status count
redxeth 0:98e98e01a6ce 775 LastLinePosition = CurrentLinePosition;
redxeth 0:98e98e01a6ce 776 CurrentLinePosition = PosEdges[0] - ( MAX_LINE_WIDTH / 2); // update line position
redxeth 0:98e98e01a6ce 777 // TERMINAL_PRINTF("*** SINGLE POSEDGE LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition);
redxeth 0:98e98e01a6ce 778 }
redxeth 0:98e98e01a6ce 779 } else if ((numNegEdges == 1) && (numPosEdges == 0)) { // 1 neg edge found (POSSIBLE LINE)
redxeth 0:98e98e01a6ce 780 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 781 {
redxeth 0:98e98e01a6ce 782 CurrentTrackStatus = LineFound; // report line found!
redxeth 0:98e98e01a6ce 783 UnknownCount = 0; // reset unknown status count
redxeth 0:98e98e01a6ce 784 LastLinePosition = CurrentLinePosition;
redxeth 0:98e98e01a6ce 785 CurrentLinePosition = NegEdges[0] + ( MAX_LINE_WIDTH / 2); // update line position
redxeth 0:98e98e01a6ce 786 // TERMINAL_PRINTF("*** SINGLE NEGEDGE LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition);
redxeth 0:98e98e01a6ce 787 }
redxeth 0:98e98e01a6ce 788 } else if ((numPosEdges == 2) && (numNegEdges == 2)) { // 2 negative and 2 positive edges found (STARTING/FINISH GATE)
redxeth 0:98e98e01a6ce 789
redxeth 0:98e98e01a6ce 790 if ((((NegEdges[0] - PosEdges[0]) >= MIN_LINE_WIDTH) && ((NegEdges[0] - PosEdges[0]) <= MAX_LINE_WIDTH)) && // white left 'line'
redxeth 0:98e98e01a6ce 791 (((NegEdges[1] - PosEdges[1]) >= MIN_LINE_WIDTH) && ((NegEdges[1] - PosEdges[1]) <= MAX_LINE_WIDTH)) && // white right 'line'
redxeth 0:98e98e01a6ce 792 (((PosEdges[1] - NegEdges[0]) >= MIN_LINE_WIDTH) && ((PosEdges[1] - NegEdges[0]) <= MAX_LINE_WIDTH)) // actual track line
redxeth 0:98e98e01a6ce 793 )
redxeth 0:98e98e01a6ce 794
redxeth 0:98e98e01a6ce 795
redxeth 0:98e98e01a6ce 796 if (startRaceTicker > STARTGATEDELAY) { // only start counting for starting gate until after delay
redxeth 0:98e98e01a6ce 797 StartGateFoundCount++;
redxeth 0:98e98e01a6ce 798 }
redxeth 0:98e98e01a6ce 799
redxeth 0:98e98e01a6ce 800 CurrentTrackStatus = StartGateFound;
redxeth 0:98e98e01a6ce 801 UnknownCount = 0; // reset unknown status count
redxeth 0:98e98e01a6ce 802
redxeth 0:98e98e01a6ce 803 } 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 804
redxeth 0:98e98e01a6ce 805 // remove edges that aren't close to center TBD DDHH
redxeth 0:98e98e01a6ce 806
redxeth 0:98e98e01a6ce 807 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 808 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 809 TERMINAL_PRINTF("********** NOT SURE FOUND ********** \r\n");
redxeth 0:98e98e01a6ce 810 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 811 }
redxeth 0:98e98e01a6ce 812 CurrentTrackStatus = Unknown;
redxeth 0:98e98e01a6ce 813
redxeth 0:98e98e01a6ce 814 } else { // no track or starting gate found
redxeth 0:98e98e01a6ce 815
redxeth 0:98e98e01a6ce 816 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 817 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 818 TERMINAL_PRINTF("*** !!!!!!!!!! LINE NOT FOUND !!!!!!! *** \r\n", CurrentLinePosition);
redxeth 0:98e98e01a6ce 819 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 820 }
redxeth 0:98e98e01a6ce 821
redxeth 0:98e98e01a6ce 822 CurrentTrackStatus = Unknown;
redxeth 0:98e98e01a6ce 823 UnknownCount++;
redxeth 0:98e98e01a6ce 824 }
redxeth 0:98e98e01a6ce 825
redxeth 0:98e98e01a6ce 826
redxeth 0:98e98e01a6ce 827
redxeth 0:98e98e01a6ce 828
redxeth 0:98e98e01a6ce 829 }
redxeth 0:98e98e01a6ce 830
redxeth 0:98e98e01a6ce 831 void ActOnTrackStatus()
redxeth 0:98e98e01a6ce 832 {
redxeth 0:98e98e01a6ce 833 // Decide what to do next based on current track status
redxeth 0:98e98e01a6ce 834
redxeth 0:98e98e01a6ce 835 if (CurrentTrackStatus == LineFound) { // LINE FOUND!
redxeth 0:98e98e01a6ce 836
redxeth 0:98e98e01a6ce 837 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 838 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 839 TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition);
redxeth 0:98e98e01a6ce 840 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 841 }
redxeth 0:98e98e01a6ce 842
redxeth 0:98e98e01a6ce 843 // Update steering position
redxeth 0:98e98e01a6ce 844 SteeringControl();
redxeth 0:98e98e01a6ce 845
redxeth 0:98e98e01a6ce 846 // Apply to servo
redxeth 0:98e98e01a6ce 847 Steer();
redxeth 0:98e98e01a6ce 848
redxeth 0:98e98e01a6ce 849 } else if (CurrentTrackStatus == StartGateFound) { // STARTING GATE FOUND
redxeth 0:98e98e01a6ce 850
redxeth 0:98e98e01a6ce 851 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 852 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 853 TERMINAL_PRINTF("********** STARTING GATE FOUND ********** \r\n");
redxeth 0:98e98e01a6ce 854 TERMINAL_PRINTF("********** count = %d ********** \r\n", StartGateFoundCount);
redxeth 0:98e98e01a6ce 855 TERMINAL_PRINTF("***************************************** \r\n");
redxeth 0:98e98e01a6ce 856 }
redxeth 0:98e98e01a6ce 857
redxeth 0:98e98e01a6ce 858 // END RACE!
redxeth 0:98e98e01a6ce 859 if (startGateStop) {
redxeth 0:98e98e01a6ce 860 if (StartGateFoundCount > STARTGATEFOUNDMAX)
redxeth 0:98e98e01a6ce 861 {
redxeth 0:98e98e01a6ce 862 go = false; // STOP!!
redxeth 0:98e98e01a6ce 863 }
redxeth 0:98e98e01a6ce 864 }
redxeth 0:98e98e01a6ce 865
redxeth 0:98e98e01a6ce 866 }
redxeth 0:98e98e01a6ce 867
redxeth 0:98e98e01a6ce 868
redxeth 0:98e98e01a6ce 869
redxeth 0:98e98e01a6ce 870 }
redxeth 0:98e98e01a6ce 871
redxeth 0:98e98e01a6ce 872 void SteeringControl()
redxeth 0:98e98e01a6ce 873 {
redxeth 0:98e98e01a6ce 874
redxeth 0:98e98e01a6ce 875 float targetPosition = (float)( (NUM_LINE_SCAN / 2) - 0.5); // target to achieve for line position
redxeth 0:98e98e01a6ce 876
redxeth 0:98e98e01a6ce 877 float KP; // proportional control factor
redxeth 0:98e98e01a6ce 878 float KI; // integral control factor
redxeth 0:98e98e01a6ce 879 float KD; // derivative control factor
redxeth 0:98e98e01a6ce 880
redxeth 0:98e98e01a6ce 881 float Pout, Iout, Dout; // PID terms
redxeth 0:98e98e01a6ce 882
redxeth 0:98e98e01a6ce 883 // Calculate error
redxeth 0:98e98e01a6ce 884 // make error to the right positive
redxeth 0:98e98e01a6ce 885 // i.e. if LINE to the right-- then CurrentLinePosError > 0
redxeth 0:98e98e01a6ce 886 // if LINE to the left -- then CurrentLinePosError < 0
redxeth 0:98e98e01a6ce 887 CurrentLinePosError = CurrentLinePosition - targetPosition;
redxeth 0:98e98e01a6ce 888
redxeth 0:98e98e01a6ce 889 // Get absolute error
redxeth 0:98e98e01a6ce 890 if (CurrentLinePosError >= 0)
redxeth 0:98e98e01a6ce 891 AbsError = CurrentLinePosError;
redxeth 0:98e98e01a6ce 892 else
redxeth 0:98e98e01a6ce 893 AbsError = -1 * CurrentLinePosError;
redxeth 0:98e98e01a6ce 894
redxeth 0:98e98e01a6ce 895 // CHOOSE SET OF PID CONTROL PARAMETERS
redxeth 0:98e98e01a6ce 896 switch (CONTROL_METHOD) {
redxeth 0:98e98e01a6ce 897 case 0:
redxeth 0:98e98e01a6ce 898 // Pure proportional control based on range of steering values vs. range of error values
redxeth 0:98e98e01a6ce 899 KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT ) / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH );
redxeth 0:98e98e01a6ce 900 KD = 0;
redxeth 0:98e98e01a6ce 901 KI = 0;
redxeth 0:98e98e01a6ce 902 break;
redxeth 0:98e98e01a6ce 903 case 1:
redxeth 0:98e98e01a6ce 904 // Proportional control with 50% bit more oomph --- a bit more aggressive around the bends
redxeth 0:98e98e01a6ce 905 KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT ) / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH );
redxeth 0:98e98e01a6ce 906 KP = KP * 1.5;
redxeth 0:98e98e01a6ce 907 KD = 0;
redxeth 0:98e98e01a6ce 908 KI = 0;
redxeth 0:98e98e01a6ce 909 break;
redxeth 0:98e98e01a6ce 910 case 2: // MANUAL TUNING CASE 1 (use pot to help determine tuning parameters)
redxeth 0:98e98e01a6ce 911 KP = TUNE_KP;
redxeth 0:98e98e01a6ce 912 KI = TUNE_KI;
redxeth 0:98e98e01a6ce 913 KD = TUNE_KD;
redxeth 0:98e98e01a6ce 914 case 3:
redxeth 0:98e98e01a6ce 915 if (AbsError < ABS_ERROR_THRESH) {
redxeth 0:98e98e01a6ce 916 KP = 0.003; // when relatively straight, keep KP gain low
redxeth 0:98e98e01a6ce 917 } else {
redxeth 0:98e98e01a6ce 918 KP = 0.010; // when curve begins or off track, increase KP gain
redxeth 0:98e98e01a6ce 919 }
redxeth 0:98e98e01a6ce 920 KI = 0;
redxeth 0:98e98e01a6ce 921 KD = 0;
redxeth 0:98e98e01a6ce 922
redxeth 0:98e98e01a6ce 923 default:
redxeth 0:98e98e01a6ce 924 break;
redxeth 0:98e98e01a6ce 925 }
redxeth 0:98e98e01a6ce 926
redxeth 0:98e98e01a6ce 927 /* Pseudocode
redxeth 0:98e98e01a6ce 928 previous_error = 0
redxeth 0:98e98e01a6ce 929 integral = 0
redxeth 0:98e98e01a6ce 930 start:
redxeth 0:98e98e01a6ce 931 error = setpoint - measured_value
redxeth 0:98e98e01a6ce 932 integral = integral + error*dt
redxeth 0:98e98e01a6ce 933 derivative = (error - previous_error)/dt
redxeth 0:98e98e01a6ce 934 output = Kp*error + Ki*integral + Kd*derivative
redxeth 0:98e98e01a6ce 935 previous_error = error
redxeth 0:98e98e01a6ce 936 wait(dt)
redxeth 0:98e98e01a6ce 937 goto start
redxeth 0:98e98e01a6ce 938 */
redxeth 0:98e98e01a6ce 939
redxeth 0:98e98e01a6ce 940
redxeth 0:98e98e01a6ce 941 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 942 TERMINAL_PRINTF("KP = %6.4f\r\n", KP);
redxeth 0:98e98e01a6ce 943 TERMINAL_PRINTF("TARGET %6.3f\r\n", targetPosition);
redxeth 0:98e98e01a6ce 944 }
redxeth 0:98e98e01a6ce 945
redxeth 0:98e98e01a6ce 946
redxeth 0:98e98e01a6ce 947
redxeth 0:98e98e01a6ce 948 // Update integral of error
redxeth 0:98e98e01a6ce 949 // i.e. if LINE stays to the right, then SumLinePosError increases
redxeth 0:98e98e01a6ce 950 // i.e. if LINE stays to the left, then SumLinePosError decreases
redxeth 0:98e98e01a6ce 951 SumLinePosError = SumLinePosError + ( CurrentLinePosError * DT );
redxeth 0:98e98e01a6ce 952
redxeth 0:98e98e01a6ce 953 DerivError = (CurrentLinePosError - LastLinePosError) / DT;
redxeth 0:98e98e01a6ce 954
redxeth 0:98e98e01a6ce 955 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 956 TERMINAL_PRINTF("CURRENT LINE POSITION %9.3f\r\n", CurrentLinePosition);
redxeth 0:98e98e01a6ce 957 TERMINAL_PRINTF("CURRENT LINE POSITION ERROR %9.3f\r\n", CurrentLinePosError);
redxeth 0:98e98e01a6ce 958 }
redxeth 0:98e98e01a6ce 959
redxeth 0:98e98e01a6ce 960 // SECOND- calculate new servo position
redxeth 0:98e98e01a6ce 961
redxeth 0:98e98e01a6ce 962 // proportional control term
redxeth 0:98e98e01a6ce 963 Pout = KP * CurrentLinePosError;
redxeth 0:98e98e01a6ce 964
redxeth 0:98e98e01a6ce 965 // integral control term
redxeth 0:98e98e01a6ce 966 Iout = KI * SumLinePosError;
redxeth 0:98e98e01a6ce 967
redxeth 0:98e98e01a6ce 968 // Derivative control term
redxeth 0:98e98e01a6ce 969 Dout = KD * DerivError;
redxeth 0:98e98e01a6ce 970
redxeth 0:98e98e01a6ce 971 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 972 TERMINAL_PRINTF("KP = %6.4f\r\n", KP);
redxeth 0:98e98e01a6ce 973 TERMINAL_PRINTF("KI = %6.4f\r\n", KI);
redxeth 0:98e98e01a6ce 974 TERMINAL_PRINTF("KD = %6.4f\r\n", KD);
redxeth 0:98e98e01a6ce 975 TERMINAL_PRINTF("Pout = %6.4f\r\n", Pout);
redxeth 0:98e98e01a6ce 976 TERMINAL_PRINTF("Iout = %6.4f\r\n", Iout);
redxeth 0:98e98e01a6ce 977 TERMINAL_PRINTF("Dout = %6.4f\r\n", Dout);
redxeth 0:98e98e01a6ce 978 }
redxeth 0:98e98e01a6ce 979
redxeth 0:98e98e01a6ce 980 // Finally add offset to steering to account for non-centered servo mounting
redxeth 0:98e98e01a6ce 981 // CurrentSteerSetting = Pout + Iout + Dout + ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 );
redxeth 0:98e98e01a6ce 982 CurrentSteerSetting = Pout + ( (float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 );
redxeth 0:98e98e01a6ce 983
redxeth 0:98e98e01a6ce 984 // store for next cycle deriv calculation
redxeth 0:98e98e01a6ce 985 LastLinePosError = CurrentLinePosError;
redxeth 0:98e98e01a6ce 986
redxeth 0:98e98e01a6ce 987 // for tuning control algo only
redxeth 0:98e98e01a6ce 988 if (1 == 0) {
redxeth 0:98e98e01a6ce 989 TERMINAL_PRINTF("*** ******************************** \r\n");
redxeth 0:98e98e01a6ce 990 TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n", CurrentLinePosition);
redxeth 0:98e98e01a6ce 991 TERMINAL_PRINTF("*** ERROR %9.3f *** \r\n", CurrentLinePosError);
redxeth 0:98e98e01a6ce 992 TERMINAL_PRINTF("*** INTEGRAL ERROR %9.3f *** \r\n", SumLinePosError);
redxeth 0:98e98e01a6ce 993 TERMINAL_PRINTF("*** DERIVATIVE ERROR %9.3f *** \r\n", DerivError);
redxeth 0:98e98e01a6ce 994 TERMINAL_PRINTF("*** P STEER SETTING %9.3f *** \r\n", CurrentSteerSetting);
redxeth 0:98e98e01a6ce 995 TERMINAL_PRINTF("*** PI STEER SETTING %9.3f *** \r\n", (CurrentSteerSetting + Iout));
redxeth 0:98e98e01a6ce 996 TERMINAL_PRINTF("*** ******************************** \r\n");
redxeth 0:98e98e01a6ce 997 wait_ms(1000);
redxeth 0:98e98e01a6ce 998 }
redxeth 0:98e98e01a6ce 999
redxeth 0:98e98e01a6ce 1000 }
redxeth 0:98e98e01a6ce 1001
redxeth 0:98e98e01a6ce 1002 void Steer()
redxeth 0:98e98e01a6ce 1003 {
redxeth 0:98e98e01a6ce 1004
redxeth 0:98e98e01a6ce 1005 // make sure doesn't go beyond steering limits
redxeth 0:98e98e01a6ce 1006 if (CurrentSteerSetting > MAX_STEER_RIGHT)
redxeth 0:98e98e01a6ce 1007 {
redxeth 0:98e98e01a6ce 1008 CurrentSteerSetting = MAX_STEER_RIGHT;
redxeth 0:98e98e01a6ce 1009 } else if (CurrentSteerSetting < MAX_STEER_LEFT)
redxeth 0:98e98e01a6ce 1010 {
redxeth 0:98e98e01a6ce 1011 CurrentSteerSetting = MAX_STEER_LEFT;
redxeth 0:98e98e01a6ce 1012 }
redxeth 0:98e98e01a6ce 1013
redxeth 0:98e98e01a6ce 1014 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 1015 TERMINAL_PRINTF("APPLYING SERVO SETTING %5.3f\r\n", CurrentSteerSetting);
redxeth 0:98e98e01a6ce 1016 }
redxeth 0:98e98e01a6ce 1017 TFC_SetServo(0,CurrentSteerSetting);
redxeth 0:98e98e01a6ce 1018
redxeth 0:98e98e01a6ce 1019 }
redxeth 0:98e98e01a6ce 1020
redxeth 0:98e98e01a6ce 1021 void SpeedControl()
redxeth 0:98e98e01a6ce 1022 {
redxeth 0:98e98e01a6ce 1023
redxeth 0:98e98e01a6ce 1024 // Get max speed setting from reading pot0
redxeth 0:98e98e01a6ce 1025 // then adjust
redxeth 0:98e98e01a6ce 1026
redxeth 0:98e98e01a6ce 1027 float ErrLimit;
redxeth 0:98e98e01a6ce 1028 float LeftDriveRatio, RightDriveRatio;
redxeth 0:98e98e01a6ce 1029
redxeth 0:98e98e01a6ce 1030 // set maximum speed allowed
redxeth 0:98e98e01a6ce 1031 switch (1)
redxeth 0:98e98e01a6ce 1032 {
redxeth 0:98e98e01a6ce 1033 case 0:
redxeth 0:98e98e01a6ce 1034 // read value off pot0
redxeth 0:98e98e01a6ce 1035 MaxSpeed = TFC_ReadPot(0);
redxeth 0:98e98e01a6ce 1036 break;
redxeth 0:98e98e01a6ce 1037 case 1:
redxeth 0:98e98e01a6ce 1038 if (doRisky)
redxeth 0:98e98e01a6ce 1039 MaxSpeed = TUNE_SPEED + 0.1;
redxeth 0:98e98e01a6ce 1040 else
redxeth 0:98e98e01a6ce 1041 MaxSpeed = TUNE_SPEED;
redxeth 0:98e98e01a6ce 1042 break;
redxeth 0:98e98e01a6ce 1043 case 2:
redxeth 0:98e98e01a6ce 1044 MaxSpeed = SUB_LIGHT_SPEED;
redxeth 0:98e98e01a6ce 1045 break;
redxeth 0:98e98e01a6ce 1046 case 3:
redxeth 0:98e98e01a6ce 1047 MaxSpeed = LIGHT_SPEED;
redxeth 0:98e98e01a6ce 1048 break;
redxeth 0:98e98e01a6ce 1049 case 4:
redxeth 0:98e98e01a6ce 1050 MaxSpeed = RIDICULOUS_SPEED;
redxeth 0:98e98e01a6ce 1051 break;
redxeth 0:98e98e01a6ce 1052 case 5:
redxeth 0:98e98e01a6ce 1053 MaxSpeed = LUDICROUS_SPEED;
redxeth 0:98e98e01a6ce 1054 break;
redxeth 0:98e98e01a6ce 1055 default:
redxeth 0:98e98e01a6ce 1056 break;
redxeth 0:98e98e01a6ce 1057 }
redxeth 0:98e98e01a6ce 1058
redxeth 0:98e98e01a6ce 1059 switch (SPEED_ADJUST)
redxeth 0:98e98e01a6ce 1060 {
redxeth 0:98e98e01a6ce 1061 case 0:
redxeth 0:98e98e01a6ce 1062 // SPEED ADJUST METHOD 0
redxeth 0:98e98e01a6ce 1063 // no speed adjust
redxeth 0:98e98e01a6ce 1064 LeftDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1065 RightDriveRatio = LeftDriveRatio;
redxeth 0:98e98e01a6ce 1066 case 1:
redxeth 0:98e98e01a6ce 1067 // SPEED ADJUST METHOD 1
redxeth 0:98e98e01a6ce 1068 // High speed when error is low, low speed when error is high
redxeth 0:98e98e01a6ce 1069 // lower speed when more than third outside of center
redxeth 0:98e98e01a6ce 1070 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.33;
redxeth 0:98e98e01a6ce 1071 if (AbsError > ErrLimit) {
redxeth 0:98e98e01a6ce 1072 LeftDriveRatio = MIN_POWER;
redxeth 0:98e98e01a6ce 1073 } else {
redxeth 0:98e98e01a6ce 1074 LeftDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1075 }
redxeth 0:98e98e01a6ce 1076 RightDriveRatio = LeftDriveRatio;
redxeth 0:98e98e01a6ce 1077 break;
redxeth 0:98e98e01a6ce 1078 case 2:
redxeth 0:98e98e01a6ce 1079 // SPEED ADJUST METHOD 2
redxeth 0:98e98e01a6ce 1080 // Have max/min speed adjust proportional to absolute value of line error
redxeth 0:98e98e01a6ce 1081 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO;
redxeth 0:98e98e01a6ce 1082 LeftDriveRatio = MAX_POWER - ((MAX_POWER - MIN_POWER) * (AbsError / ErrLimit));
redxeth 0:98e98e01a6ce 1083 RightDriveRatio = LeftDriveRatio;
redxeth 0:98e98e01a6ce 1084 break;
redxeth 0:98e98e01a6ce 1085 case 3:
redxeth 0:98e98e01a6ce 1086 // SPEED ADJUST METHOD 3
redxeth 0:98e98e01a6ce 1087 // have wheel relative speed proportional to absolute value of line error
redxeth 0:98e98e01a6ce 1088 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO;
redxeth 0:98e98e01a6ce 1089 if (CurrentLinePosError > 0) { // heading right
redxeth 0:98e98e01a6ce 1090 LeftDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1091 RightDriveRatio = (MIN_POWER - MAX_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ) + MAX_POWER;
redxeth 0:98e98e01a6ce 1092 } else if (CurrentLinePosError < 0) { // heading left
redxeth 0:98e98e01a6ce 1093 RightDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1094 LeftDriveRatio = (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) ) + MAX_POWER;
redxeth 0:98e98e01a6ce 1095 } else {
redxeth 0:98e98e01a6ce 1096 LeftDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1097 RightDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1098 }
redxeth 0:98e98e01a6ce 1099 break;
redxeth 0:98e98e01a6ce 1100 case 4:
redxeth 0:98e98e01a6ce 1101 // SPEED ADJUST METHOD 4
redxeth 0:98e98e01a6ce 1102 // have wheel relative speed proportional to absolute value of line error
redxeth 0:98e98e01a6ce 1103 // only when above a certain error
redxeth 0:98e98e01a6ce 1104 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.1;
redxeth 0:98e98e01a6ce 1105 if (CurrentLinePosError > ErrLimit) { // heading right
redxeth 0:98e98e01a6ce 1106 LeftDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) );
redxeth 0:98e98e01a6ce 1107 RightDriveRatio = MIN_POWER;
redxeth 0:98e98e01a6ce 1108 } else if (CurrentLinePosError < (-1 * ErrLimit)) { // heading left
redxeth 0:98e98e01a6ce 1109 RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER) * (CurrentLinePosError * 2 / ( (float) RANGE ) );
redxeth 0:98e98e01a6ce 1110 LeftDriveRatio = MIN_POWER;
redxeth 0:98e98e01a6ce 1111 } else {
redxeth 0:98e98e01a6ce 1112 LeftDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1113 RightDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1114 }
redxeth 0:98e98e01a6ce 1115 break;
redxeth 0:98e98e01a6ce 1116 case 5:
redxeth 0:98e98e01a6ce 1117 // SPEED ADJUST METHOD 5
redxeth 0:98e98e01a6ce 1118 // High speed when error is low, low speed when error is high
redxeth 0:98e98e01a6ce 1119 // lower speed when more than third outside of center
redxeth 0:98e98e01a6ce 1120 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.2;
redxeth 0:98e98e01a6ce 1121 if (AbsError > ErrLimit) {
redxeth 0:98e98e01a6ce 1122 LeftDriveRatio = MIN_POWER;
redxeth 0:98e98e01a6ce 1123 } else {
redxeth 0:98e98e01a6ce 1124 LeftDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1125 }
redxeth 0:98e98e01a6ce 1126 RightDriveRatio = LeftDriveRatio;
redxeth 0:98e98e01a6ce 1127 break;
redxeth 0:98e98e01a6ce 1128 case 6:
redxeth 0:98e98e01a6ce 1129 // SPEED ADJUST METHOD 6
redxeth 0:98e98e01a6ce 1130 // High speed when error is low, low speed when error is high
redxeth 0:98e98e01a6ce 1131 // lower speed when more than third outside of center
redxeth 0:98e98e01a6ce 1132 if (AbsError > ABS_ERROR_THRESH) {
redxeth 0:98e98e01a6ce 1133 LeftDriveRatio = MIN_POWER;
redxeth 0:98e98e01a6ce 1134 } else {
redxeth 0:98e98e01a6ce 1135 LeftDriveRatio = MAX_POWER;
redxeth 0:98e98e01a6ce 1136 }
redxeth 0:98e98e01a6ce 1137 RightDriveRatio = LeftDriveRatio;
redxeth 0:98e98e01a6ce 1138 break;
redxeth 0:98e98e01a6ce 1139 default:
redxeth 0:98e98e01a6ce 1140 break;
redxeth 0:98e98e01a6ce 1141
redxeth 0:98e98e01a6ce 1142 }
redxeth 0:98e98e01a6ce 1143 // TBD-- add speed adjust based on Xaccel sensor!
redxeth 0:98e98e01a6ce 1144
redxeth 0:98e98e01a6ce 1145
redxeth 0:98e98e01a6ce 1146 // currently no control mechanism as don't have speed sensor
redxeth 0:98e98e01a6ce 1147 CurrentLeftDriveSetting = (float) (LeftDriveRatio / 100) * MaxSpeed;
redxeth 0:98e98e01a6ce 1148 CurrentRightDriveSetting = (float) (RightDriveRatio / 100) * MaxSpeed;
redxeth 0:98e98e01a6ce 1149
redxeth 0:98e98e01a6ce 1150
redxeth 0:98e98e01a6ce 1151 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 1152 TERMINAL_PRINTF("Abs Error: %4.2f\r\n", AbsError);
redxeth 0:98e98e01a6ce 1153 TERMINAL_PRINTF("Error Limit: %4.2f\r\n", ErrLimit);
redxeth 0:98e98e01a6ce 1154 TERMINAL_PRINTF("MAX SPEED = %5.2f\n", MaxSpeed);
redxeth 0:98e98e01a6ce 1155 TERMINAL_PRINTF("Current Left Drive Setting: %5.2f\r\n", CurrentLeftDriveSetting);
redxeth 0:98e98e01a6ce 1156 TERMINAL_PRINTF("Current Right Drive Setting: %5.2f\r\n", CurrentRightDriveSetting);
redxeth 0:98e98e01a6ce 1157 }
redxeth 0:98e98e01a6ce 1158 if (1 == 0) {
redxeth 0:98e98e01a6ce 1159 TERMINAL_PRINTF("Current Left Drive Setting: %5.2f\r\n", CurrentLeftDriveSetting);
redxeth 0:98e98e01a6ce 1160 TERMINAL_PRINTF("Current Right Drive Setting: %5.2f\r\n", CurrentRightDriveSetting);
redxeth 0:98e98e01a6ce 1161 }
redxeth 0:98e98e01a6ce 1162
redxeth 0:98e98e01a6ce 1163 }
redxeth 0:98e98e01a6ce 1164
redxeth 0:98e98e01a6ce 1165 void Drive()
redxeth 0:98e98e01a6ce 1166 {
redxeth 0:98e98e01a6ce 1167
redxeth 0:98e98e01a6ce 1168 // START!
redxeth 0:98e98e01a6ce 1169 // if not going, go when button A is pressed
redxeth 0:98e98e01a6ce 1170 if (!go) {
redxeth 0:98e98e01a6ce 1171 if(TFC_PUSH_BUTTON_0_PRESSED) {
redxeth 0:98e98e01a6ce 1172 go = true;
redxeth 0:98e98e01a6ce 1173 UnknownCount = 0;
redxeth 0:98e98e01a6ce 1174 StartGateFoundCount = 0;
redxeth 0:98e98e01a6ce 1175 startRaceTicker = TFC_Ticker[0]; // keep track of start of race
redxeth 0:98e98e01a6ce 1176 logDataIndex = 0; // reset log data index
redxeth 0:98e98e01a6ce 1177 }
redxeth 0:98e98e01a6ce 1178 }
redxeth 0:98e98e01a6ce 1179
redxeth 0:98e98e01a6ce 1180 // STOP!
redxeth 0:98e98e01a6ce 1181 // if going, stop when button A is pressed
redxeth 0:98e98e01a6ce 1182 if (go) {
redxeth 0:98e98e01a6ce 1183 if(TFC_PUSH_BUTTON_1_PRESSED) {
redxeth 0:98e98e01a6ce 1184 go = false;
redxeth 0:98e98e01a6ce 1185 StartGateFoundCount = 0;
redxeth 0:98e98e01a6ce 1186 }
redxeth 0:98e98e01a6ce 1187 }
redxeth 0:98e98e01a6ce 1188
redxeth 0:98e98e01a6ce 1189 // EMERGENCY STOP!
redxeth 0:98e98e01a6ce 1190 // 'kill switch' to prevent crashes off-track
redxeth 0:98e98e01a6ce 1191 if (killSwitch) {
redxeth 0:98e98e01a6ce 1192 if (UnknownCount > UNKNOWN_COUNT_MAX) { // if track not found after certain time
redxeth 0:98e98e01a6ce 1193 go = false; // kill engine
redxeth 0:98e98e01a6ce 1194 StartGateFoundCount = 0;
redxeth 0:98e98e01a6ce 1195 }
redxeth 0:98e98e01a6ce 1196 }
redxeth 0:98e98e01a6ce 1197
redxeth 0:98e98e01a6ce 1198 // ****************
redxeth 0:98e98e01a6ce 1199
redxeth 0:98e98e01a6ce 1200 if (!go) { // stop!
redxeth 0:98e98e01a6ce 1201 TFC_SetMotorPWM(0,0); //Make sure motors are off
redxeth 0:98e98e01a6ce 1202 TFC_HBRIDGE_DISABLE;
redxeth 0:98e98e01a6ce 1203 }
redxeth 0:98e98e01a6ce 1204
redxeth 0:98e98e01a6ce 1205 if (go) { // go!
redxeth 0:98e98e01a6ce 1206 TFC_HBRIDGE_ENABLE;
redxeth 0:98e98e01a6ce 1207 // motor A = right, motor B = left based on way it is mounted
redxeth 0:98e98e01a6ce 1208 TFC_SetMotorPWM(CurrentRightDriveSetting,CurrentLeftDriveSetting);
redxeth 0:98e98e01a6ce 1209 }
redxeth 0:98e98e01a6ce 1210 }
redxeth 0:98e98e01a6ce 1211
redxeth 0:98e98e01a6ce 1212
redxeth 0:98e98e01a6ce 1213 void adjustLights()
redxeth 0:98e98e01a6ce 1214 {
redxeth 0:98e98e01a6ce 1215
redxeth 0:98e98e01a6ce 1216 // LIGHT ADJUST METHOD 1
redxeth 0:98e98e01a6ce 1217 // threshold is 1/5 of light intensity 'range'
redxeth 0:98e98e01a6ce 1218 if (1 == 0) {
redxeth 0:98e98e01a6ce 1219 DerivThreshold = (float) (MaxLightIntensity - MinLightIntensity) / 5;
redxeth 0:98e98e01a6ce 1220 NegDerivThreshold = (float) -1 * (DerivThreshold);
redxeth 0:98e98e01a6ce 1221 PosDerivThreshold = (float) (DerivThreshold);
redxeth 0:98e98e01a6ce 1222 } else {
redxeth 0:98e98e01a6ce 1223 // LIGHT ADJUST METHOD 2 -- SEEMS TO WORK MUCH BETTER
redxeth 0:98e98e01a6ce 1224 // pos edge threshold is half range of max deriv above aver derive
redxeth 0:98e98e01a6ce 1225 // neg edge threshold is half range of min deriv above aver derive
redxeth 0:98e98e01a6ce 1226
redxeth 0:98e98e01a6ce 1227 NegDerivThreshold = (float) (minDerVal - aveDerVal) * DER_RATIO;
redxeth 0:98e98e01a6ce 1228 PosDerivThreshold = (float) (maxDerVal - aveDerVal) * DER_RATIO;
redxeth 0:98e98e01a6ce 1229
redxeth 0:98e98e01a6ce 1230 }
redxeth 0:98e98e01a6ce 1231
redxeth 0:98e98e01a6ce 1232 printAdjustLightsData();
redxeth 0:98e98e01a6ce 1233
redxeth 0:98e98e01a6ce 1234 }
redxeth 0:98e98e01a6ce 1235
redxeth 0:98e98e01a6ce 1236 void printAdjustLightsData()
redxeth 0:98e98e01a6ce 1237 {
redxeth 0:98e98e01a6ce 1238 if (terminalOutput == 3) {
redxeth 0:98e98e01a6ce 1239 TERMINAL_PRINTF("Max Light Intensity: %4d\r\n", MaxLightIntensity);
redxeth 0:98e98e01a6ce 1240 TERMINAL_PRINTF("Min Light Intensity: %4d\r\n", MinLightIntensity);
redxeth 0:98e98e01a6ce 1241 TERMINAL_PRINTF("Deriv Threshold: %9.3f\r\n", DerivThreshold);
redxeth 0:98e98e01a6ce 1242 }
redxeth 0:98e98e01a6ce 1243
redxeth 0:98e98e01a6ce 1244 }
redxeth 0:98e98e01a6ce 1245
redxeth 0:98e98e01a6ce 1246 void feedbackLights()
redxeth 0:98e98e01a6ce 1247 {
redxeth 0:98e98e01a6ce 1248 switch (CurrentTrackStatus)
redxeth 0:98e98e01a6ce 1249 {
redxeth 0:98e98e01a6ce 1250 case LineFound:
redxeth 0:98e98e01a6ce 1251 TFC_BAT_LED0_OFF;
redxeth 0:98e98e01a6ce 1252 TFC_BAT_LED1_ON;
redxeth 0:98e98e01a6ce 1253 TFC_BAT_LED2_ON;
redxeth 0:98e98e01a6ce 1254 TFC_BAT_LED3_OFF;
redxeth 0:98e98e01a6ce 1255 break;
redxeth 0:98e98e01a6ce 1256 case StartGateFound:
redxeth 0:98e98e01a6ce 1257 TFC_BAT_LED0_ON;
redxeth 0:98e98e01a6ce 1258 TFC_BAT_LED1_OFF;
redxeth 0:98e98e01a6ce 1259 TFC_BAT_LED2_OFF;
redxeth 0:98e98e01a6ce 1260 TFC_BAT_LED3_ON;
redxeth 0:98e98e01a6ce 1261 break;
redxeth 0:98e98e01a6ce 1262 default:
redxeth 0:98e98e01a6ce 1263 TFC_BAT_LED0_OFF;
redxeth 0:98e98e01a6ce 1264 TFC_BAT_LED1_OFF;
redxeth 0:98e98e01a6ce 1265 TFC_BAT_LED2_OFF;
redxeth 0:98e98e01a6ce 1266 TFC_BAT_LED3_OFF;
redxeth 0:98e98e01a6ce 1267 }
redxeth 0:98e98e01a6ce 1268
redxeth 0:98e98e01a6ce 1269 }
redxeth 0:98e98e01a6ce 1270