Keith Cyr / TFC-Mentoring-Matters-Abstraction

TFC-Mentoring-Matters-Abstraction

Dependencies:   FRDM-TFC

RaceCar/TheDetails.cpp@0:0699eb71778e, 2014-07-16 (annotated)

Committer:
mrkeithcyr
Date:
Wed Jul 16 15:36:33 2014 +0000
Revision:
0:0699eb71778e
First code commit.; Note known  issue: in modes 1, 4, and 5 - The motor outputs are severely retarded by serial connection.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mrkeithcyr 0:0699eb71778e 1 #include "TFC.h"
mrkeithcyr 0:0699eb71778e 2 #include "mbed.h"
mrkeithcyr 0:0699eb71778e 3
mrkeithcyr 0:0699eb71778e 4 /****** INCLUDES ******/
mrkeithcyr 0:0699eb71778e 5
mrkeithcyr 0:0699eb71778e 6 #include "mbed.h"
mrkeithcyr 0:0699eb71778e 7 #include "common.h"
mrkeithcyr 0:0699eb71778e 8 #include "TFC.h"
mrkeithcyr 0:0699eb71778e 9
mrkeithcyr 0:0699eb71778e 10 /**********************/
mrkeithcyr 0:0699eb71778e 11
mrkeithcyr 0:0699eb71778e 12
mrkeithcyr 0:0699eb71778e 13 /****** CAMERA PARAMETERS ******/
mrkeithcyr 0:0699eb71778e 14
mrkeithcyr 0:0699eb71778e 15 #define NUM_LINE_SCAN 128
mrkeithcyr 0:0699eb71778e 16 #define MAX_LINE_SCAN NUM_LINE_SCAN-1
mrkeithcyr 0:0699eb71778e 17 #define MIN_LINE_WIDTH 0
mrkeithcyr 0:0699eb71778e 18 #define MAX_LINE_WIDTH 15
mrkeithcyr 0:0699eb71778e 19
mrkeithcyr 0:0699eb71778e 20 // number of pixels at end of camera data to ignore
mrkeithcyr 0:0699eb71778e 21 // set to 0 for now, later make 15
mrkeithcyr 0:0699eb71778e 22 #define FILTER_ENDS 15
mrkeithcyr 0:0699eb71778e 23
mrkeithcyr 0:0699eb71778e 24 // range of camera pixels to consider
mrkeithcyr 0:0699eb71778e 25 #define RANGE (NUM_LINE_SCAN - (2 * FILTER_ENDS))
mrkeithcyr 0:0699eb71778e 26
mrkeithcyr 0:0699eb71778e 27 // ratio of max possible error to pixels (have to measure!)
mrkeithcyr 0:0699eb71778e 28 #define ERR_RATIO 0.85
mrkeithcyr 0:0699eb71778e 29
mrkeithcyr 0:0699eb71778e 30 // ratio for der threshold level (was 0.5 initially, may put back)
mrkeithcyr 0:0699eb71778e 31 #define DER_RATIO 0.5
mrkeithcyr 0:0699eb71778e 32
mrkeithcyr 0:0699eb71778e 33 /*******************************/
mrkeithcyr 0:0699eb71778e 34
mrkeithcyr 0:0699eb71778e 35
mrkeithcyr 0:0699eb71778e 36 /****** STEERING AND SERVO PARAMETERS ******/
mrkeithcyr 0:0699eb71778e 37
mrkeithcyr 0:0699eb71778e 38 // value determined by garage mode 2 measure
mrkeithcyr 0:0699eb71778e 39 // must be adjusted for every servo horn attached
mrkeithcyr 0:0699eb71778e 40 // must be re-adjusted (or at least checked) if
mrkeithcyr 0:0699eb71778e 41 #define MAX_STEER_LEFT -0.38
mrkeithcyr 0:0699eb71778e 42
mrkeithcyr 0:0699eb71778e 43 // value determined by garage mode 2 measure
mrkeithcyr 0:0699eb71778e 44 #define MAX_STEER_RIGHT 0.49
mrkeithcyr 0:0699eb71778e 45
mrkeithcyr 0:0699eb71778e 46 // # MS of time between intervals (doesn't really matter)
mrkeithcyr 0:0699eb71778e 47 #define DT 0.02
mrkeithcyr 0:0699eb71778e 48
mrkeithcyr 0:0699eb71778e 49 /*******************************************/
mrkeithcyr 0:0699eb71778e 50
mrkeithcyr 0:0699eb71778e 51
mrkeithcyr 0:0699eb71778e 52 /****** LOGGING PARAMETERS ******/
mrkeithcyr 0:0699eb71778e 53
mrkeithcyr 0:0699eb71778e 54 // number of frames to log (when logging is active) ~14 sec worth!
mrkeithcyr 0:0699eb71778e 55 #define NUM_LOG_FRAMES 700
mrkeithcyr 0:0699eb71778e 56
mrkeithcyr 0:0699eb71778e 57 /********************************/
mrkeithcyr 0:0699eb71778e 58
mrkeithcyr 0:0699eb71778e 59
mrkeithcyr 0:0699eb71778e 60 /****** FOR DEBUG TUNING ******/
mrkeithcyr 0:0699eb71778e 61
mrkeithcyr 0:0699eb71778e 62 #define TUNE_KP 0.008
mrkeithcyr 0:0699eb71778e 63 #define TUNE_KI 0
mrkeithcyr 0:0699eb71778e 64 #define TUNE_KD 0
mrkeithcyr 0:0699eb71778e 65
mrkeithcyr 0:0699eb71778e 66 // percent minimum power
mrkeithcyr 0:0699eb71778e 67 // estimating for a 2-ft turn => 24" / (24" + 6" car) = 4/5
mrkeithcyr 0:0699eb71778e 68 // speed of inner wheel is 20% lower worst case
mrkeithcyr 0:0699eb71778e 69 #define MIN_POWER 60
mrkeithcyr 0:0699eb71778e 70
mrkeithcyr 0:0699eb71778e 71 // do not change
mrkeithcyr 0:0699eb71778e 72 #define SPEED_ADJUST 4
mrkeithcyr 0:0699eb71778e 73
mrkeithcyr 0:0699eb71778e 74 // number of pixels line position offset before changing KP value
mrkeithcyr 0:0699eb71778e 75 #define ABS_ERROR_THRESH 10
mrkeithcyr 0:0699eb71778e 76
mrkeithcyr 0:0699eb71778e 77 // which control method to use
mrkeithcyr 0:0699eb71778e 78 #define CONTROL_METHOD 1
mrkeithcyr 0:0699eb71778e 79
mrkeithcyr 0:0699eb71778e 80 /******************************/
mrkeithcyr 0:0699eb71778e 81
mrkeithcyr 0:0699eb71778e 82
mrkeithcyr 0:0699eb71778e 83 /****** DRIVE/MOTOR PARAMETERS ******/
mrkeithcyr 0:0699eb71778e 84
mrkeithcyr 0:0699eb71778e 85 // maximum speed cap for slow speed setting
mrkeithcyr 0:0699eb71778e 86 #define LIGHT_SPEED 0.5
mrkeithcyr 0:0699eb71778e 87
mrkeithcyr 0:0699eb71778e 88 // maximum speed cap for faster speed setting
mrkeithcyr 0:0699eb71778e 89 #define LUDICROUS_SPEED 0.7
mrkeithcyr 0:0699eb71778e 90
mrkeithcyr 0:0699eb71778e 91 // percent max power (for speed adjustments)
mrkeithcyr 0:0699eb71778e 92 #define MAX_POWER 100
mrkeithcyr 0:0699eb71778e 93
mrkeithcyr 0:0699eb71778e 94 /************************************/
mrkeithcyr 0:0699eb71778e 95
mrkeithcyr 0:0699eb71778e 96
mrkeithcyr 0:0699eb71778e 97 /****** ALGORITHM PARAMETERS ******/
mrkeithcyr 0:0699eb71778e 98
mrkeithcyr 0:0699eb71778e 99 // max value to allow for unknown track conditions before killing engine
mrkeithcyr 0:0699eb71778e 100 #define UNKNOWN_COUNT_MAX 50
mrkeithcyr 0:0699eb71778e 101
mrkeithcyr 0:0699eb71778e 102 // max value to allow for finding starting gate before killing engine
mrkeithcyr 0:0699eb71778e 103 #define STARTGATEFOUNDMAX 0
mrkeithcyr 0:0699eb71778e 104
mrkeithcyr 0:0699eb71778e 105 // delay before searching for starting gate to kill engine
mrkeithcyr 0:0699eb71778e 106 #define STARTGATEDELAY 50
mrkeithcyr 0:0699eb71778e 107
mrkeithcyr 0:0699eb71778e 108 /**********************************/
mrkeithcyr 0:0699eb71778e 109
mrkeithcyr 0:0699eb71778e 110
mrkeithcyr 0:0699eb71778e 111 /****** IMAGE PROCESSING VARIABLES ******/
mrkeithcyr 0:0699eb71778e 112
mrkeithcyr 0:0699eb71778e 113 // snapshot of camera data for this 'frame'
mrkeithcyr 0:0699eb71778e 114 uint16_t GrabLineScanImage0[NUM_LINE_SCAN];
mrkeithcyr 0:0699eb71778e 115
mrkeithcyr 0:0699eb71778e 116 // derivative of line scan data
mrkeithcyr 0:0699eb71778e 117 float DerivLineScanImage0[NUM_LINE_SCAN];
mrkeithcyr 0:0699eb71778e 118
mrkeithcyr 0:0699eb71778e 119 // array-- set of where in line scan data negative edges found
mrkeithcyr 0:0699eb71778e 120 float NegEdges[NUM_LINE_SCAN];
mrkeithcyr 0:0699eb71778e 121
mrkeithcyr 0:0699eb71778e 122 // array-- set of where in line scan data positive edges found
mrkeithcyr 0:0699eb71778e 123 float PosEdges[NUM_LINE_SCAN];
mrkeithcyr 0:0699eb71778e 124
mrkeithcyr 0:0699eb71778e 125 // max value of valid neg and positive indices
mrkeithcyr 0:0699eb71778e 126 // also serves as a count of # edges found
mrkeithcyr 0:0699eb71778e 127 uint16_t numNegEdges = 0, numPosEdges = 0;
mrkeithcyr 0:0699eb71778e 128
mrkeithcyr 0:0699eb71778e 129 // maximum measured light intensity -- to account for lighting differences
mrkeithcyr 0:0699eb71778e 130 uint16_t MaxLightIntensity = 0;
mrkeithcyr 0:0699eb71778e 131
mrkeithcyr 0:0699eb71778e 132 // minimum measured light intensity -- to account for lighting differences
mrkeithcyr 0:0699eb71778e 133 uint16_t MinLightIntensity = (1 << 12);
mrkeithcyr 0:0699eb71778e 134
mrkeithcyr 0:0699eb71778e 135 // maximum derivative value
mrkeithcyr 0:0699eb71778e 136 float maxDerVal = 0;
mrkeithcyr 0:0699eb71778e 137
mrkeithcyr 0:0699eb71778e 138 // minimum derivative value
mrkeithcyr 0:0699eb71778e 139 float minDerVal = (float) (1 << 12);
mrkeithcyr 0:0699eb71778e 140
mrkeithcyr 0:0699eb71778e 141 // average derivative value
mrkeithcyr 0:0699eb71778e 142 float aveDerVal = 0;
mrkeithcyr 0:0699eb71778e 143
mrkeithcyr 0:0699eb71778e 144 // default derivative threshold
mrkeithcyr 0:0699eb71778e 145 float DerivThreshold = (1 << 9);
mrkeithcyr 0:0699eb71778e 146
mrkeithcyr 0:0699eb71778e 147 // default positive edge derivative threshold
mrkeithcyr 0:0699eb71778e 148 float PosDerivThreshold = (1 << 9);
mrkeithcyr 0:0699eb71778e 149
mrkeithcyr 0:0699eb71778e 150 // default negative edge derivative threshold
mrkeithcyr 0:0699eb71778e 151 float NegDerivThreshold = (1 << 9);
mrkeithcyr 0:0699eb71778e 152
mrkeithcyr 0:0699eb71778e 153 /****************************************/
mrkeithcyr 0:0699eb71778e 154
mrkeithcyr 0:0699eb71778e 155
mrkeithcyr 0:0699eb71778e 156 /****** STEERING CONTROL VARIABLES ******/
mrkeithcyr 0:0699eb71778e 157
mrkeithcyr 0:0699eb71778e 158 // current position of track line (in pixels -- 0 to 127)
mrkeithcyr 0:0699eb71778e 159 float CurrentLinePosition;
mrkeithcyr 0:0699eb71778e 160
mrkeithcyr 0:0699eb71778e 161 // last position of track line (in pixels -- 0 to 127)
mrkeithcyr 0:0699eb71778e 162 float LastLinePosition;
mrkeithcyr 0:0699eb71778e 163
mrkeithcyr 0:0699eb71778e 164 // current line position error (used for derivative calculation)
mrkeithcyr 0:0699eb71778e 165 float CurrentLinePosError = 0;
mrkeithcyr 0:0699eb71778e 166 float AbsError;
mrkeithcyr 0:0699eb71778e 167
mrkeithcyr 0:0699eb71778e 168 // last line position error (used for derivative calculation)
mrkeithcyr 0:0699eb71778e 169 float LastLinePosError = 0;
mrkeithcyr 0:0699eb71778e 170
mrkeithcyr 0:0699eb71778e 171 // sum of line position error (used for integral calculation)
mrkeithcyr 0:0699eb71778e 172 float SumLinePosError = 0;
mrkeithcyr 0:0699eb71778e 173
mrkeithcyr 0:0699eb71778e 174 // derivative of the error
mrkeithcyr 0:0699eb71778e 175 float DerivError = 0;
mrkeithcyr 0:0699eb71778e 176
mrkeithcyr 0:0699eb71778e 177 // drive straight at first
mrkeithcyr 0:0699eb71778e 178 float CurrentSteerSetting = (MAX_STEER_RIGHT + MAX_STEER_LEFT) / 2;
mrkeithcyr 0:0699eb71778e 179
mrkeithcyr 0:0699eb71778e 180 // left wheel drive setting
mrkeithcyr 0:0699eb71778e 181 float CurrentLeftDriveSetting = 0;
mrkeithcyr 0:0699eb71778e 182
mrkeithcyr 0:0699eb71778e 183 // right wheel drive setting
mrkeithcyr 0:0699eb71778e 184 float CurrentRightDriveSetting = 0;
mrkeithcyr 0:0699eb71778e 185
mrkeithcyr 0:0699eb71778e 186 /****************************************/
mrkeithcyr 0:0699eb71778e 187
mrkeithcyr 0:0699eb71778e 188
mrkeithcyr 0:0699eb71778e 189 /****** SPEED CONTROL VARIABLES ******/
mrkeithcyr 0:0699eb71778e 190
mrkeithcyr 0:0699eb71778e 191 // maximum speed allowed
mrkeithcyr 0:0699eb71778e 192 float MaxSpeed;
mrkeithcyr 0:0699eb71778e 193
mrkeithcyr 0:0699eb71778e 194 // ticker at start of race
mrkeithcyr 0:0699eb71778e 195 uint16_t startRaceTicker;
mrkeithcyr 0:0699eb71778e 196
mrkeithcyr 0:0699eb71778e 197 /*************************************/
mrkeithcyr 0:0699eb71778e 198
mrkeithcyr 0:0699eb71778e 199
mrkeithcyr 0:0699eb71778e 200 /****** CUSTOM DATA TYPES ******/
mrkeithcyr 0:0699eb71778e 201
mrkeithcyr 0:0699eb71778e 202 typedef enum TrackStatusType {Unknown,
mrkeithcyr 0:0699eb71778e 203 LineFound,
mrkeithcyr 0:0699eb71778e 204 StartGateFound,
mrkeithcyr 0:0699eb71778e 205 LineJustLeft} TrackStatusType;
mrkeithcyr 0:0699eb71778e 206
mrkeithcyr 0:0699eb71778e 207 TrackStatusType CurrentTrackStatus; // current track status
mrkeithcyr 0:0699eb71778e 208 TrackStatusType LastTrackStatus; // last track status
mrkeithcyr 0:0699eb71778e 209
mrkeithcyr 0:0699eb71778e 210 /* typedef enum TrackType {NotSure,
mrkeithcyr 0:0699eb71778e 211 Straight,
mrkeithcyr 0:0699eb71778e 212 Curve,
mrkeithcyr 0:0699eb71778e 213 Wiggle,
mrkeithcyr 0:0699eb71778e 214 Bumps,
mrkeithcyr 0:0699eb71778e 215 StartGate,
mrkeithcyr 0:0699eb71778e 216 UpHill,
mrkeithcyr 0:0699eb71778e 217 DownHill} TrackType;
mrkeithcyr 0:0699eb71778e 218
mrkeithcyr 0:0699eb71778e 219 TrackType CurrentTrack; */
mrkeithcyr 0:0699eb71778e 220
mrkeithcyr 0:0699eb71778e 221 struct LogData {
mrkeithcyr 0:0699eb71778e 222 float linepos;
mrkeithcyr 0:0699eb71778e 223 float steersetting;
mrkeithcyr 0:0699eb71778e 224 float leftdrivesetting;
mrkeithcyr 0:0699eb71778e 225 float rightdrivesetting;
mrkeithcyr 0:0699eb71778e 226 };
mrkeithcyr 0:0699eb71778e 227
mrkeithcyr 0:0699eb71778e 228 // array of log data to store
mrkeithcyr 0:0699eb71778e 229 LogData frameLogs[NUM_LOG_FRAMES];
mrkeithcyr 0:0699eb71778e 230
mrkeithcyr 0:0699eb71778e 231 // index for log data
mrkeithcyr 0:0699eb71778e 232 int logDataIndex;
mrkeithcyr 0:0699eb71778e 233
mrkeithcyr 0:0699eb71778e 234 // how many times start gate has been found
mrkeithcyr 0:0699eb71778e 235 int StartGateFoundCount = 0;
mrkeithcyr 0:0699eb71778e 236
mrkeithcyr 0:0699eb71778e 237 // how many times nothing has been found
mrkeithcyr 0:0699eb71778e 238 // help5 with kill switch implementation
mrkeithcyr 0:0699eb71778e 239 int UnknownCount = 0;
mrkeithcyr 0:0699eb71778e 240
mrkeithcyr 0:0699eb71778e 241 // Car can go! Should be set to false to start.
mrkeithcyr 0:0699eb71778e 242 bool go = false;
mrkeithcyr 0:0699eb71778e 243
mrkeithcyr 0:0699eb71778e 244 /*******************************/
mrkeithcyr 0:0699eb71778e 245
mrkeithcyr 0:0699eb71778e 246 /****** EXTRA CONTROL PARAMETERS ******/
mrkeithcyr 0:0699eb71778e 247
mrkeithcyr 0:0699eb71778e 248 // if true, ignores real camera and uses fake camera input instead
mrkeithcyr 0:0699eb71778e 249 // used for data processing debug
mrkeithcyr 0:0699eb71778e 250 extern bool debugFakeMode = false;
mrkeithcyr 0:0699eb71778e 251
mrkeithcyr 0:0699eb71778e 252 // whether output terminal data
mrkeithcyr 0:0699eb71778e 253 bool terminalOutput = false;
mrkeithcyr 0:0699eb71778e 254
mrkeithcyr 0:0699eb71778e 255 // whether to capture log data to output later on
mrkeithcyr 0:0699eb71778e 256 bool doLogData = false;
mrkeithcyr 0:0699eb71778e 257
mrkeithcyr 0:0699eb71778e 258 // whether to enable Kill Switch
mrkeithcyr 0:0699eb71778e 259 // allows engine to stop after not finding track if true
mrkeithcyr 0:0699eb71778e 260 bool killSwitch = false;
mrkeithcyr 0:0699eb71778e 261
mrkeithcyr 0:0699eb71778e 262 // whether to stop or not depending on starting gate reading
mrkeithcyr 0:0699eb71778e 263 bool startGateStop = false;
mrkeithcyr 0:0699eb71778e 264
mrkeithcyr 0:0699eb71778e 265 // race style -- whether conservative or risky
mrkeithcyr 0:0699eb71778e 266 bool doRisky = false;
mrkeithcyr 0:0699eb71778e 267
mrkeithcyr 0:0699eb71778e 268 /**************************************/
mrkeithcyr 0:0699eb71778e 269
mrkeithcyr 0:0699eb71778e 270 /****** terminalMode() *********************************************************
mrkeithcyr 0:0699eb71778e 271 Purpose: Checks the current setting of dip switch 4.
mrkeithcyr 0:0699eb71778e 272 Parameters: None
mrkeithcyr 0:0699eb71778e 273 Returns: True if dip switch 4 is set.
mrkeithcyr 0:0699eb71778e 274 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 275 bool terminalMode()
mrkeithcyr 0:0699eb71778e 276 {
mrkeithcyr 0:0699eb71778e 277 return ((TFC_GetDIP_Switch()>>3)&0x01 == 1);
mrkeithcyr 0:0699eb71778e 278 }
mrkeithcyr 0:0699eb71778e 279
mrkeithcyr 0:0699eb71778e 280 /****** GiveDriveSettingsToMotors() ********************************************
mrkeithcyr 0:0699eb71778e 281 Purpose: Simply passes parameters to TFC_SetMotorPWM().
mrkeithcyr 0:0699eb71778e 282 Parameters: The desired settings for each of the two motors
mrkeithcyr 0:0699eb71778e 283 Returns: None
mrkeithcyr 0:0699eb71778e 284 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 285 void GiveDriveSettingsToMotors(float right_drive_setting,
mrkeithcyr 0:0699eb71778e 286 float left_drive_setting)
mrkeithcyr 0:0699eb71778e 287 {
mrkeithcyr 0:0699eb71778e 288 TFC_SetMotorPWM(right_drive_setting, left_drive_setting);
mrkeithcyr 0:0699eb71778e 289 }
mrkeithcyr 0:0699eb71778e 290
mrkeithcyr 0:0699eb71778e 291 /****** GiveSteeringSettingTo() ************************************************
mrkeithcyr 0:0699eb71778e 292 Purpose: Simply passes parameters to TFC_SetServo().
mrkeithcyr 0:0699eb71778e 293 Parameters: The servo number and the steering setting.
mrkeithcyr 0:0699eb71778e 294 Returns: None
mrkeithcyr 0:0699eb71778e 295 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 296 void GiveSteeringSettingTo(uint8_t ServoNumber, float Position)
mrkeithcyr 0:0699eb71778e 297 {
mrkeithcyr 0:0699eb71778e 298 TFC_SetServo(ServoNumber, Position);
mrkeithcyr 0:0699eb71778e 299 }
mrkeithcyr 0:0699eb71778e 300
mrkeithcyr 0:0699eb71778e 301 /****** ResetCamera() **********************************************************
mrkeithcyr 0:0699eb71778e 302 Purpose: This function resets the camera after an image has been used.
mrkeithcyr 0:0699eb71778e 303 Parameters: None
mrkeithcyr 0:0699eb71778e 304 Returns: None
mrkeithcyr 0:0699eb71778e 305 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 306 void ResetCamera()
mrkeithcyr 0:0699eb71778e 307 {
mrkeithcyr 0:0699eb71778e 308 TFC_LineScanImageReady = false;
mrkeithcyr 0:0699eb71778e 309 }
mrkeithcyr 0:0699eb71778e 310
mrkeithcyr 0:0699eb71778e 311 /****** ResetMaximumLightReading() *********************************************
mrkeithcyr 0:0699eb71778e 312 Purpose: Resets the maximum light intensity variable
mrkeithcyr 0:0699eb71778e 313 Parameters: None
mrkeithcyr 0:0699eb71778e 314 Returns: None
mrkeithcyr 0:0699eb71778e 315 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 316 void ResetMaximumLightReading()
mrkeithcyr 0:0699eb71778e 317 {
mrkeithcyr 0:0699eb71778e 318 MaxLightIntensity = 0;
mrkeithcyr 0:0699eb71778e 319 }
mrkeithcyr 0:0699eb71778e 320
mrkeithcyr 0:0699eb71778e 321 /****** ResetMinimumLightReading() *********************************************
mrkeithcyr 0:0699eb71778e 322 Purpose: Resets the minimum light intensity variable
mrkeithcyr 0:0699eb71778e 323 Parameters: None
mrkeithcyr 0:0699eb71778e 324 Returns: None
mrkeithcyr 0:0699eb71778e 325 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 326 void ResetMinimumLightReading()
mrkeithcyr 0:0699eb71778e 327 {
mrkeithcyr 0:0699eb71778e 328 MinLightIntensity = (1 << 12);
mrkeithcyr 0:0699eb71778e 329 }
mrkeithcyr 0:0699eb71778e 330
mrkeithcyr 0:0699eb71778e 331 /****** grabCameraFrame() ******************************************************
mrkeithcyr 0:0699eb71778e 332 Purpose: This function gets a scan from the camera or fakes a scan from the
mrkeithcyr 0:0699eb71778e 333 camera. There are different types of fake scans that can be used to test
mrkeithcyr 0:0699eb71778e 334 other parts of the car to see how the car reacts to certain types of line
mrkeithcyr 0:0699eb71778e 335 conditions.
mrkeithcyr 0:0699eb71778e 336 Parameters: None
mrkeithcyr 0:0699eb71778e 337 Returns: None
mrkeithcyr 0:0699eb71778e 338 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 339 void grabCameraFrame()
mrkeithcyr 0:0699eb71778e 340 {
mrkeithcyr 0:0699eb71778e 341 uint32_t i = 0;
mrkeithcyr 0:0699eb71778e 342 uint8_t fake_type = 4; // type of fake data if used
mrkeithcyr 0:0699eb71778e 343
mrkeithcyr 0:0699eb71778e 344 for(i=0;i<NUM_LINE_SCAN;i++) // print one line worth of data
mrkeithcyr 0:0699eb71778e 345 // (128 characters) from Camera 0
mrkeithcyr 0:0699eb71778e 346 {
mrkeithcyr 0:0699eb71778e 347 if (debugFakeMode)
mrkeithcyr 0:0699eb71778e 348 { // use fake camera data
mrkeithcyr 0:0699eb71778e 349 switch (fake_type)
mrkeithcyr 0:0699eb71778e 350 {
mrkeithcyr 0:0699eb71778e 351 // ideal track -- line in center
mrkeithcyr 0:0699eb71778e 352 case 0:
mrkeithcyr 0:0699eb71778e 353 if (i<57 || i > 70)
mrkeithcyr 0:0699eb71778e 354 GrabLineScanImage0[i] = 0xFFF; // no line
mrkeithcyr 0:0699eb71778e 355 else
mrkeithcyr 0:0699eb71778e 356 GrabLineScanImage0[i] = 0x4B0; // line
mrkeithcyr 0:0699eb71778e 357 break;
mrkeithcyr 0:0699eb71778e 358 // ideal track -- line to the left
mrkeithcyr 0:0699eb71778e 359 case 1:
mrkeithcyr 0:0699eb71778e 360 if (i<27 || i > 40)
mrkeithcyr 0:0699eb71778e 361 GrabLineScanImage0[i] = 0xFFF; // no line
mrkeithcyr 0:0699eb71778e 362 else
mrkeithcyr 0:0699eb71778e 363 GrabLineScanImage0[i] = 0x4B0; // line
mrkeithcyr 0:0699eb71778e 364 break;
mrkeithcyr 0:0699eb71778e 365 // ideal track -- line to the right
mrkeithcyr 0:0699eb71778e 366 case 2:
mrkeithcyr 0:0699eb71778e 367 if (i<87 || i > 100)
mrkeithcyr 0:0699eb71778e 368 GrabLineScanImage0[i] = 0xFFF; // no line
mrkeithcyr 0:0699eb71778e 369 else
mrkeithcyr 0:0699eb71778e 370 GrabLineScanImage0[i] = 0x4B0; // line
mrkeithcyr 0:0699eb71778e 371 break;
mrkeithcyr 0:0699eb71778e 372 // ideal track -- starting gate!
mrkeithcyr 0:0699eb71778e 373 case 3:
mrkeithcyr 0:0699eb71778e 374 // TBD
mrkeithcyr 0:0699eb71778e 375 break;
mrkeithcyr 0:0699eb71778e 376 // less than ideal track -- debug multi-edge issue!
mrkeithcyr 0:0699eb71778e 377 case 4:
mrkeithcyr 0:0699eb71778e 378 if (i<54)
mrkeithcyr 0:0699eb71778e 379 GrabLineScanImage0[i] = 4000; // no line
mrkeithcyr 0:0699eb71778e 380 if (i == 54)
mrkeithcyr 0:0699eb71778e 381 GrabLineScanImage0[i] = 3370; // neg edge
mrkeithcyr 0:0699eb71778e 382 if (i == 55)
mrkeithcyr 0:0699eb71778e 383 GrabLineScanImage0[i] = 3309; // neg edge
mrkeithcyr 0:0699eb71778e 384 if (i == 56)
mrkeithcyr 0:0699eb71778e 385 GrabLineScanImage0[i] = 2016; // neg edge
mrkeithcyr 0:0699eb71778e 386 if (i == 57)
mrkeithcyr 0:0699eb71778e 387 GrabLineScanImage0[i] = 711; // neg edge
mrkeithcyr 0:0699eb71778e 388 if (i == 58)
mrkeithcyr 0:0699eb71778e 389 GrabLineScanImage0[i] = 696; // neg edge
mrkeithcyr 0:0699eb71778e 390 if ((i>58) && (i<69))
mrkeithcyr 0:0699eb71778e 391 GrabLineScanImage0[i] = 500; // line
mrkeithcyr 0:0699eb71778e 392 if (i == 69)
mrkeithcyr 0:0699eb71778e 393 GrabLineScanImage0[i] = 1800; // pos edge
mrkeithcyr 0:0699eb71778e 394 if (i > 69)
mrkeithcyr 0:0699eb71778e 395 GrabLineScanImage0[i] = 4000; // no line
mrkeithcyr 0:0699eb71778e 396 default:
mrkeithcyr 0:0699eb71778e 397 break;
mrkeithcyr 0:0699eb71778e 398 }
mrkeithcyr 0:0699eb71778e 399 } else // use real camera data
mrkeithcyr 0:0699eb71778e 400 {
mrkeithcyr 0:0699eb71778e 401 GrabLineScanImage0[i] = TFC_LineScanImage0[i];
mrkeithcyr 0:0699eb71778e 402 }
mrkeithcyr 0:0699eb71778e 403 }
mrkeithcyr 0:0699eb71778e 404 }
mrkeithcyr 0:0699eb71778e 405
mrkeithcyr 0:0699eb71778e 406 /****** GetTheImageToUse() *****************************************************
mrkeithcyr 0:0699eb71778e 407 Purpose: Retrieves whichever image we are supposed to use.
mrkeithcyr 0:0699eb71778e 408 Parameters: None
mrkeithcyr 0:0699eb71778e 409 Returns: None
mrkeithcyr 0:0699eb71778e 410 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 411 void GetTheImageToUse()
mrkeithcyr 0:0699eb71778e 412 {
mrkeithcyr 0:0699eb71778e 413 grabCameraFrame();
mrkeithcyr 0:0699eb71778e 414 }
mrkeithcyr 0:0699eb71778e 415
mrkeithcyr 0:0699eb71778e 416 /****** derivativeLineScan() ***************************************************
mrkeithcyr 0:0699eb71778e 417 Purpose: Calculates derivative of line scan data.
mrkeithcyr 0:0699eb71778e 418 Parameters: Pointer to current image scan data, and a pointer to where you want
mrkeithcyr 0:0699eb71778e 419 the derivative to be output.
mrkeithcyr 0:0699eb71778e 420 Returns: None
mrkeithcyr 0:0699eb71778e 421 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 422 void derivativeLineScan(uint16_t* LineScanDataIn, float* DerivLineScanDataOut)
mrkeithcyr 0:0699eb71778e 423 {
mrkeithcyr 0:0699eb71778e 424 uint32_t i;
mrkeithcyr 0:0699eb71778e 425 uint32_t minCnt = 0;
mrkeithcyr 0:0699eb71778e 426 uint32_t maxCnt = 0;
mrkeithcyr 0:0699eb71778e 427 float DerVal;
mrkeithcyr 0:0699eb71778e 428 float upperDerVal;
mrkeithcyr 0:0699eb71778e 429 float lowerDerVal = 0;
mrkeithcyr 0:0699eb71778e 430
mrkeithcyr 0:0699eb71778e 431 maxDerVal = 0;
mrkeithcyr 0:0699eb71778e 432 minDerVal = (float) (1 << 12);
mrkeithcyr 0:0699eb71778e 433 aveDerVal = 0;
mrkeithcyr 0:0699eb71778e 434
mrkeithcyr 0:0699eb71778e 435 minCnt = FILTER_ENDS;
mrkeithcyr 0:0699eb71778e 436 maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
mrkeithcyr 0:0699eb71778e 437
mrkeithcyr 0:0699eb71778e 438 // TERMINAL_PRINTF("i, upperDerVal, lowerDerVal, DerVal\r\n");
mrkeithcyr 0:0699eb71778e 439
mrkeithcyr 0:0699eb71778e 440 // print one line worth of data from Camera 0
mrkeithcyr 0:0699eb71778e 441 for(i=minCnt;i<maxCnt;i++)
mrkeithcyr 0:0699eb71778e 442 {
mrkeithcyr 0:0699eb71778e 443 // store max light intensity value
mrkeithcyr 0:0699eb71778e 444 if (LineScanDataIn[i] > MaxLightIntensity)
mrkeithcyr 0:0699eb71778e 445 MaxLightIntensity = LineScanDataIn[i];
mrkeithcyr 0:0699eb71778e 446
mrkeithcyr 0:0699eb71778e 447 // store min light intensity value
mrkeithcyr 0:0699eb71778e 448 if (LineScanDataIn[i] < MinLightIntensity)
mrkeithcyr 0:0699eb71778e 449 MinLightIntensity = LineScanDataIn[i];
mrkeithcyr 0:0699eb71778e 450
mrkeithcyr 0:0699eb71778e 451 // Central Derivative
mrkeithcyr 0:0699eb71778e 452 // start point
mrkeithcyr 0:0699eb71778e 453 if (i==minCnt)
mrkeithcyr 0:0699eb71778e 454 {
mrkeithcyr 0:0699eb71778e 455 upperDerVal = (float)(LineScanDataIn[i+1]);
mrkeithcyr 0:0699eb71778e 456 // make same as start point
mrkeithcyr 0:0699eb71778e 457 lowerDerVal = (float)(LineScanDataIn[i]);
mrkeithcyr 0:0699eb71778e 458 }
mrkeithcyr 0:0699eb71778e 459 // end point
mrkeithcyr 0:0699eb71778e 460 else if (i==maxCnt - 1)
mrkeithcyr 0:0699eb71778e 461 {
mrkeithcyr 0:0699eb71778e 462 // make same as end point
mrkeithcyr 0:0699eb71778e 463 upperDerVal = (float)(LineScanDataIn[i]);
mrkeithcyr 0:0699eb71778e 464 lowerDerVal = (float)(LineScanDataIn[i-1]);
mrkeithcyr 0:0699eb71778e 465 }
mrkeithcyr 0:0699eb71778e 466 // any other point
mrkeithcyr 0:0699eb71778e 467 else
mrkeithcyr 0:0699eb71778e 468 {
mrkeithcyr 0:0699eb71778e 469 upperDerVal = (float)(LineScanDataIn[i+1]);
mrkeithcyr 0:0699eb71778e 470 lowerDerVal = (float)(LineScanDataIn[i-1]);
mrkeithcyr 0:0699eb71778e 471 }
mrkeithcyr 0:0699eb71778e 472 DerVal = (upperDerVal - lowerDerVal) / 2;
mrkeithcyr 0:0699eb71778e 473 // TERMINAL_PRINTF("%d,%9.3f,%9.3f,%9.3f\r\n",
mrkeithcyr 0:0699eb71778e 474 // i, upperDerVal, lowerDerVal, DerVal);
mrkeithcyr 0:0699eb71778e 475
mrkeithcyr 0:0699eb71778e 476 if (DerVal > maxDerVal)
mrkeithcyr 0:0699eb71778e 477 {
mrkeithcyr 0:0699eb71778e 478 maxDerVal = DerVal;
mrkeithcyr 0:0699eb71778e 479 }
mrkeithcyr 0:0699eb71778e 480 if (DerVal < minDerVal)
mrkeithcyr 0:0699eb71778e 481 {
mrkeithcyr 0:0699eb71778e 482 minDerVal = DerVal;
mrkeithcyr 0:0699eb71778e 483 }
mrkeithcyr 0:0699eb71778e 484 //get sum
mrkeithcyr 0:0699eb71778e 485 aveDerVal = aveDerVal + DerVal;
mrkeithcyr 0:0699eb71778e 486 DerivLineScanDataOut[i] = DerVal;
mrkeithcyr 0:0699eb71778e 487 }
mrkeithcyr 0:0699eb71778e 488 aveDerVal = (float) aveDerVal / (maxCnt - minCnt);
mrkeithcyr 0:0699eb71778e 489 }
mrkeithcyr 0:0699eb71778e 490
mrkeithcyr 0:0699eb71778e 491 /****** GetCameraData() *****************************************************
mrkeithcyr 0:0699eb71778e 492 Purpose: Processes an image to get the camera's raw data.
mrkeithcyr 0:0699eb71778e 493 Parameters: None
mrkeithcyr 0:0699eb71778e 494 Returns: None
mrkeithcyr 0:0699eb71778e 495 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 496 void GetCameraData()
mrkeithcyr 0:0699eb71778e 497 {
mrkeithcyr 0:0699eb71778e 498 derivativeLineScan(&GrabLineScanImage0[0], &DerivLineScanImage0[0]);
mrkeithcyr 0:0699eb71778e 499 }
mrkeithcyr 0:0699eb71778e 500
mrkeithcyr 0:0699eb71778e 501 /****** printAdjustLightsData() ************************************************
mrkeithcyr 0:0699eb71778e 502 Purpose: Prints out light intensity data to the terminal.
mrkeithcyr 0:0699eb71778e 503 Parameters: None
mrkeithcyr 0:0699eb71778e 504 Returns: None
mrkeithcyr 0:0699eb71778e 505 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 506 void printAdjustLightsData()
mrkeithcyr 0:0699eb71778e 507 {
mrkeithcyr 0:0699eb71778e 508 if (terminalOutput)
mrkeithcyr 0:0699eb71778e 509 {
mrkeithcyr 0:0699eb71778e 510 TERMINAL_PRINTF("Max Light Intensity: %4d\r\n", MaxLightIntensity);
mrkeithcyr 0:0699eb71778e 511 TERMINAL_PRINTF("Min Light Intensity: %4d\r\n", MinLightIntensity);
mrkeithcyr 0:0699eb71778e 512 TERMINAL_PRINTF("Deriv Threshold: %9.3f\r\n", DerivThreshold);
mrkeithcyr 0:0699eb71778e 513 }
mrkeithcyr 0:0699eb71778e 514 }
mrkeithcyr 0:0699eb71778e 515
mrkeithcyr 0:0699eb71778e 516 /****** adjustLights() *********************************************************
mrkeithcyr 0:0699eb71778e 517 Purpose: Adjusts the line found threshold for different lighting conditions.
mrkeithcyr 0:0699eb71778e 518 Parameters: None
mrkeithcyr 0:0699eb71778e 519 Returns: None
mrkeithcyr 0:0699eb71778e 520 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 521 void adjustLights()
mrkeithcyr 0:0699eb71778e 522 {
mrkeithcyr 0:0699eb71778e 523 // LIGHT ADJUST METHOD 1
mrkeithcyr 0:0699eb71778e 524 // threshold is 1/5 of light intensity 'range'
mrkeithcyr 0:0699eb71778e 525 if (1 == 0) {
mrkeithcyr 0:0699eb71778e 526 DerivThreshold = (float) (MaxLightIntensity - MinLightIntensity) / 5;
mrkeithcyr 0:0699eb71778e 527 NegDerivThreshold = (float) -1 * (DerivThreshold);
mrkeithcyr 0:0699eb71778e 528 PosDerivThreshold = (float) (DerivThreshold);
mrkeithcyr 0:0699eb71778e 529 }
mrkeithcyr 0:0699eb71778e 530
mrkeithcyr 0:0699eb71778e 531 // LIGHT ADJUST METHOD 2 -- SEEMS TO WORK MUCH BETTER
mrkeithcyr 0:0699eb71778e 532 // pos edge threshold is half range of max derivative above ave derivative
mrkeithcyr 0:0699eb71778e 533 // neg edge threshold is half range of min derivative above ave derivative
mrkeithcyr 0:0699eb71778e 534 else
mrkeithcyr 0:0699eb71778e 535 {
mrkeithcyr 0:0699eb71778e 536 NegDerivThreshold = (float) (minDerVal - aveDerVal) * DER_RATIO;
mrkeithcyr 0:0699eb71778e 537 PosDerivThreshold = (float) (maxDerVal - aveDerVal) * DER_RATIO;
mrkeithcyr 0:0699eb71778e 538 }
mrkeithcyr 0:0699eb71778e 539 printAdjustLightsData();
mrkeithcyr 0:0699eb71778e 540 }
mrkeithcyr 0:0699eb71778e 541
mrkeithcyr 0:0699eb71778e 542 /****** findEdges_v2() *********************************************************
mrkeithcyr 0:0699eb71778e 543 Purpose: This function is used to determine where the edges of the line are
mrkeithcyr 0:0699eb71778e 544 found in the camera image. The location of the line edges is printed out to
mrkeithcyr 0:0699eb71778e 545 the terminal if terminal output has been enabled.
mrkeithcyr 0:0699eb71778e 546 Parameters: Pointer to the array of line scan data derivatives
mrkeithcyr 0:0699eb71778e 547 Returns: None
mrkeithcyr 0:0699eb71778e 548 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 549 void findEdges_v2(float* derivLineScanData)
mrkeithcyr 0:0699eb71778e 550 {
mrkeithcyr 0:0699eb71778e 551 // search for edges in deriviative data using a threshold
mrkeithcyr 0:0699eb71778e 552 // need to store in a hash if that's possible...
mrkeithcyr 0:0699eb71778e 553 // combine edges that are a pixel apart
mrkeithcyr 0:0699eb71778e 554
mrkeithcyr 0:0699eb71778e 555 int i;
mrkeithcyr 0:0699eb71778e 556
mrkeithcyr 0:0699eb71778e 557 // serves as buffer to store neg edges found next to each other
mrkeithcyr 0:0699eb71778e 558 int NegEdgeBufCnt = 0, NegEdgeBufSum = 0;
mrkeithcyr 0:0699eb71778e 559 // serves as buffer to store pos edges found next to each other
mrkeithcyr 0:0699eb71778e 560 int PosEdgeBufCnt = 0, PosEdgeBufSum = 0;
mrkeithcyr 0:0699eb71778e 561
mrkeithcyr 0:0699eb71778e 562 int minCnt = FILTER_ENDS;
mrkeithcyr 0:0699eb71778e 563 int maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
mrkeithcyr 0:0699eb71778e 564
mrkeithcyr 0:0699eb71778e 565
mrkeithcyr 0:0699eb71778e 566 // count of neg edges found thus far
mrkeithcyr 0:0699eb71778e 567 numNegEdges = 0;
mrkeithcyr 0:0699eb71778e 568 // count of pos edges found thus far
mrkeithcyr 0:0699eb71778e 569 numPosEdges = 0;
mrkeithcyr 0:0699eb71778e 570 // print one line worth of data from Camera 0
mrkeithcyr 0:0699eb71778e 571 for(i=minCnt;i<maxCnt;i++)
mrkeithcyr 0:0699eb71778e 572 {
mrkeithcyr 0:0699eb71778e 573 // NEGATIVE EDGE FOUND!
mrkeithcyr 0:0699eb71778e 574 if (derivLineScanData[i] <= NegDerivThreshold)
mrkeithcyr 0:0699eb71778e 575 {
mrkeithcyr 0:0699eb71778e 576 if (terminalOutput)
mrkeithcyr 0:0699eb71778e 577 {
mrkeithcyr 0:0699eb71778e 578 TERMINAL_PRINTF("NEG EDGE FOUND AT INDEX "
mrkeithcyr 0:0699eb71778e 579 "%d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
mrkeithcyr 0:0699eb71778e 580 }
mrkeithcyr 0:0699eb71778e 581 // add value to neg edge buffer
mrkeithcyr 0:0699eb71778e 582 NegEdgeBufCnt++;
mrkeithcyr 0:0699eb71778e 583 NegEdgeBufSum = NegEdgeBufSum + i;
mrkeithcyr 0:0699eb71778e 584 }
mrkeithcyr 0:0699eb71778e 585 // POSITIVE EDGE FOUND!
mrkeithcyr 0:0699eb71778e 586 else if(derivLineScanData[i] > PosDerivThreshold)
mrkeithcyr 0:0699eb71778e 587 {
mrkeithcyr 0:0699eb71778e 588 if (terminalOutput)
mrkeithcyr 0:0699eb71778e 589 {
mrkeithcyr 0:0699eb71778e 590 TERMINAL_PRINTF("POS EDGE FOUND AT INDEX "
mrkeithcyr 0:0699eb71778e 591 "%d WITH VALUE %9.3f\r\n", i, derivLineScanData[i]);
mrkeithcyr 0:0699eb71778e 592 }
mrkeithcyr 0:0699eb71778e 593
mrkeithcyr 0:0699eb71778e 594 // add value to pos edge buffer
mrkeithcyr 0:0699eb71778e 595 PosEdgeBufCnt++;
mrkeithcyr 0:0699eb71778e 596 PosEdgeBufSum = PosEdgeBufSum + i;
mrkeithcyr 0:0699eb71778e 597
mrkeithcyr 0:0699eb71778e 598 }
mrkeithcyr 0:0699eb71778e 599
mrkeithcyr 0:0699eb71778e 600 // NO EDGE FOUND
mrkeithcyr 0:0699eb71778e 601 else
mrkeithcyr 0:0699eb71778e 602 {
mrkeithcyr 0:0699eb71778e 603 // POP EDGE BUFFERS IF NON-EMPTY AND STORE TO EDGE "STACK"
mrkeithcyr 0:0699eb71778e 604 // (i.e. edges found)
mrkeithcyr 0:0699eb71778e 605 if (NegEdgeBufCnt > 0)
mrkeithcyr 0:0699eb71778e 606 {
mrkeithcyr 0:0699eb71778e 607 // store edge value
mrkeithcyr 0:0699eb71778e 608 numNegEdges++;
mrkeithcyr 0:0699eb71778e 609 NegEdges[numNegEdges - 1] =
mrkeithcyr 0:0699eb71778e 610 (float) NegEdgeBufSum / NegEdgeBufCnt;
mrkeithcyr 0:0699eb71778e 611
mrkeithcyr 0:0699eb71778e 612 // clear edge buffer
mrkeithcyr 0:0699eb71778e 613 NegEdgeBufSum = 0; NegEdgeBufCnt = 0;
mrkeithcyr 0:0699eb71778e 614 }
mrkeithcyr 0:0699eb71778e 615
mrkeithcyr 0:0699eb71778e 616 if (PosEdgeBufCnt > 0)
mrkeithcyr 0:0699eb71778e 617 {
mrkeithcyr 0:0699eb71778e 618 // store edge value
mrkeithcyr 0:0699eb71778e 619 numPosEdges++;
mrkeithcyr 0:0699eb71778e 620 PosEdges[numPosEdges - 1] =
mrkeithcyr 0:0699eb71778e 621 (float) PosEdgeBufSum / PosEdgeBufCnt;
mrkeithcyr 0:0699eb71778e 622
mrkeithcyr 0:0699eb71778e 623 // clear edge buffer
mrkeithcyr 0:0699eb71778e 624 PosEdgeBufSum = 0; PosEdgeBufCnt = 0;
mrkeithcyr 0:0699eb71778e 625 }
mrkeithcyr 0:0699eb71778e 626 }
mrkeithcyr 0:0699eb71778e 627 }
mrkeithcyr 0:0699eb71778e 628 }
mrkeithcyr 0:0699eb71778e 629
mrkeithcyr 0:0699eb71778e 630 /****** FindTheLine() *********************************************************
mrkeithcyr 0:0699eb71778e 631 Purpose: Simply a link to whichever version of the edge-finding functions you
mrkeithcyr 0:0699eb71778e 632 choose.
mrkeithcyr 0:0699eb71778e 633 Parameters: None
mrkeithcyr 0:0699eb71778e 634 Returns: None
mrkeithcyr 0:0699eb71778e 635 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 636 void FindTheLine()
mrkeithcyr 0:0699eb71778e 637 {
mrkeithcyr 0:0699eb71778e 638 findEdges_v2(&DerivLineScanImage0[0]);
mrkeithcyr 0:0699eb71778e 639 }
mrkeithcyr 0:0699eb71778e 640
mrkeithcyr 0:0699eb71778e 641 /****** reviewEdges() **********************************************************
mrkeithcyr 0:0699eb71778e 642 Purpose: This function check which edges were found and decides whether or not
mrkeithcyr 0:0699eb71778e 643 the line has been found. Its results are printed out to the terminal if
mrkeithcyr 0:0699eb71778e 644 terminal output has been enabled.
mrkeithcyr 0:0699eb71778e 645 Parameters: None
mrkeithcyr 0:0699eb71778e 646 Returns: None
mrkeithcyr 0:0699eb71778e 647 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 648 void reviewEdges()
mrkeithcyr 0:0699eb71778e 649 {
mrkeithcyr 0:0699eb71778e 650 LastTrackStatus = CurrentTrackStatus;
mrkeithcyr 0:0699eb71778e 651
mrkeithcyr 0:0699eb71778e 652 // only one negative and positive edge found (LINE)
mrkeithcyr 0:0699eb71778e 653 if ((numPosEdges == 1) && (numNegEdges == 1))
mrkeithcyr 0:0699eb71778e 654 {
mrkeithcyr 0:0699eb71778e 655 // has proper expected width
mrkeithcyr 0:0699eb71778e 656 if (((PosEdges[0] - NegEdges[0]) >= MIN_LINE_WIDTH)
mrkeithcyr 0:0699eb71778e 657 && ((PosEdges[0] - NegEdges[0]) <= MAX_LINE_WIDTH))
mrkeithcyr 0:0699eb71778e 658 {
mrkeithcyr 0:0699eb71778e 659 // report line found!
mrkeithcyr 0:0699eb71778e 660 CurrentTrackStatus = LineFound;
mrkeithcyr 0:0699eb71778e 661 // reset unknown status count
mrkeithcyr 0:0699eb71778e 662 UnknownCount = 0;
mrkeithcyr 0:0699eb71778e 663 LastLinePosition = CurrentLinePosition;
mrkeithcyr 0:0699eb71778e 664 // update line position
mrkeithcyr 0:0699eb71778e 665 CurrentLinePosition = (PosEdges[0]+NegEdges[0]) / 2;
mrkeithcyr 0:0699eb71778e 666 }
mrkeithcyr 0:0699eb71778e 667 }
mrkeithcyr 0:0699eb71778e 668
mrkeithcyr 0:0699eb71778e 669 // 1 pos edge found (POSSIBLE LINE)
mrkeithcyr 0:0699eb71778e 670 else if ((numPosEdges == 1) && (numNegEdges == 0))
mrkeithcyr 0:0699eb71778e 671 {
mrkeithcyr 0:0699eb71778e 672 // pos edge is within line width of edge of camera (LEFT)
mrkeithcyr 0:0699eb71778e 673 if ((PosEdges[0] <= MAX_LINE_WIDTH) && (LastLinePosError < 0))
mrkeithcyr 0:0699eb71778e 674 {
mrkeithcyr 0:0699eb71778e 675 // report line found!
mrkeithcyr 0:0699eb71778e 676 CurrentTrackStatus = LineFound;
mrkeithcyr 0:0699eb71778e 677 // reset unknown status count
mrkeithcyr 0:0699eb71778e 678 UnknownCount = 0;
mrkeithcyr 0:0699eb71778e 679 LastLinePosition = CurrentLinePosition;
mrkeithcyr 0:0699eb71778e 680 // update line position
mrkeithcyr 0:0699eb71778e 681 CurrentLinePosition = PosEdges[0] - ( MAX_LINE_WIDTH / 2);
mrkeithcyr 0:0699eb71778e 682 // TERMINAL_PRINTF("*** SINGLE POSEDGE LINE FOUND AT POSITION "
mrkeithcyr 0:0699eb71778e 683 // "%9.3f *** \r\n", CurrentLinePosition);
mrkeithcyr 0:0699eb71778e 684 }
mrkeithcyr 0:0699eb71778e 685 }
mrkeithcyr 0:0699eb71778e 686
mrkeithcyr 0:0699eb71778e 687 // 1 neg edge found (POSSIBLE LINE)
mrkeithcyr 0:0699eb71778e 688 else if ((numNegEdges == 1) && (numPosEdges == 0))
mrkeithcyr 0:0699eb71778e 689 {
mrkeithcyr 0:0699eb71778e 690 // neg edge is within line width of edge of camera (RIGHT)
mrkeithcyr 0:0699eb71778e 691 if ((NegEdges[0] >= (MAX_LINE_SCAN - MAX_LINE_WIDTH))
mrkeithcyr 0:0699eb71778e 692 && (LastLinePosError > 0))
mrkeithcyr 0:0699eb71778e 693 {
mrkeithcyr 0:0699eb71778e 694 // report line found!
mrkeithcyr 0:0699eb71778e 695 CurrentTrackStatus = LineFound;
mrkeithcyr 0:0699eb71778e 696 // reset unknown status count
mrkeithcyr 0:0699eb71778e 697 UnknownCount = 0;
mrkeithcyr 0:0699eb71778e 698 LastLinePosition = CurrentLinePosition;
mrkeithcyr 0:0699eb71778e 699 // update line position
mrkeithcyr 0:0699eb71778e 700 CurrentLinePosition = NegEdges[0] + ( MAX_LINE_WIDTH / 2);
mrkeithcyr 0:0699eb71778e 701 // TERMINAL_PRINTF("*** SINGLE NEGEDGE LINE FOUND AT POSITION "
mrkeithcyr 0:0699eb71778e 702 // "%9.3f *** \r\n", CurrentLinePosition);
mrkeithcyr 0:0699eb71778e 703 }
mrkeithcyr 0:0699eb71778e 704 }
mrkeithcyr 0:0699eb71778e 705
mrkeithcyr 0:0699eb71778e 706 // 2 negative and 2 positive edges found (STARTING/FINISH GATE)
mrkeithcyr 0:0699eb71778e 707 else if ((numPosEdges == 2) && (numNegEdges == 2))
mrkeithcyr 0:0699eb71778e 708 {
mrkeithcyr 0:0699eb71778e 709 if ( // white left 'line'
mrkeithcyr 0:0699eb71778e 710 (((NegEdges[0] - PosEdges[0]) >= MIN_LINE_WIDTH)
mrkeithcyr 0:0699eb71778e 711 && ((NegEdges[0] - PosEdges[0]) <= MAX_LINE_WIDTH))
mrkeithcyr 0:0699eb71778e 712 // white right 'line'
mrkeithcyr 0:0699eb71778e 713 && (((NegEdges[1] - PosEdges[1]) >= MIN_LINE_WIDTH)
mrkeithcyr 0:0699eb71778e 714 && ((NegEdges[1] - PosEdges[1]) <= MAX_LINE_WIDTH))
mrkeithcyr 0:0699eb71778e 715 // actual track line
mrkeithcyr 0:0699eb71778e 716 && (((PosEdges[1] - NegEdges[0]) >= MIN_LINE_WIDTH)
mrkeithcyr 0:0699eb71778e 717 && ((PosEdges[1] - NegEdges[0]) <= MAX_LINE_WIDTH)))
mrkeithcyr 0:0699eb71778e 718 {
mrkeithcyr 0:0699eb71778e 719 // only start counting for starting gate until after delay
mrkeithcyr 0:0699eb71778e 720 if (startRaceTicker > STARTGATEDELAY)
mrkeithcyr 0:0699eb71778e 721 {
mrkeithcyr 0:0699eb71778e 722 StartGateFoundCount++;
mrkeithcyr 0:0699eb71778e 723 }
mrkeithcyr 0:0699eb71778e 724 }
mrkeithcyr 0:0699eb71778e 725 CurrentTrackStatus = StartGateFound;
mrkeithcyr 0:0699eb71778e 726 // reset unknown status count
mrkeithcyr 0:0699eb71778e 727 UnknownCount = 0;
mrkeithcyr 0:0699eb71778e 728 }
mrkeithcyr 0:0699eb71778e 729
mrkeithcyr 0:0699eb71778e 730 // more than 1 negative edge and positive edge found
mrkeithcyr 0:0699eb71778e 731 // (but not 2 for both) (STARTING / FINISH GATE)
mrkeithcyr 0:0699eb71778e 732 else if ((numPosEdges > 1) && (numNegEdges > 1))
mrkeithcyr 0:0699eb71778e 733 {
mrkeithcyr 0:0699eb71778e 734
mrkeithcyr 0:0699eb71778e 735 // remove edges that aren't close to center TBD DDHH
mrkeithcyr 0:0699eb71778e 736 if (terminalOutput)
mrkeithcyr 0:0699eb71778e 737 {
mrkeithcyr 0:0699eb71778e 738 TERMINAL_PRINTF("***************************************** \r\n");
mrkeithcyr 0:0699eb71778e 739 TERMINAL_PRINTF("********** NOT SURE FOUND ********** \r\n");
mrkeithcyr 0:0699eb71778e 740 TERMINAL_PRINTF("***************************************** \r\n");
mrkeithcyr 0:0699eb71778e 741 }
mrkeithcyr 0:0699eb71778e 742 CurrentTrackStatus = Unknown;
mrkeithcyr 0:0699eb71778e 743
mrkeithcyr 0:0699eb71778e 744 }
mrkeithcyr 0:0699eb71778e 745
mrkeithcyr 0:0699eb71778e 746 // no track or starting gate found
mrkeithcyr 0:0699eb71778e 747 else
mrkeithcyr 0:0699eb71778e 748 {
mrkeithcyr 0:0699eb71778e 749 if (terminalOutput) {
mrkeithcyr 0:0699eb71778e 750 TERMINAL_PRINTF("***************************************** \r\n");
mrkeithcyr 0:0699eb71778e 751 TERMINAL_PRINTF("*** !!!!!!!!!! LINE NOT FOUND !!!!!!! *** \r\n",
mrkeithcyr 0:0699eb71778e 752 CurrentLinePosition);
mrkeithcyr 0:0699eb71778e 753 TERMINAL_PRINTF("***************************************** \r\n");
mrkeithcyr 0:0699eb71778e 754 }
mrkeithcyr 0:0699eb71778e 755
mrkeithcyr 0:0699eb71778e 756 CurrentTrackStatus = Unknown;
mrkeithcyr 0:0699eb71778e 757 UnknownCount++;
mrkeithcyr 0:0699eb71778e 758 }
mrkeithcyr 0:0699eb71778e 759 }
mrkeithcyr 0:0699eb71778e 760
mrkeithcyr 0:0699eb71778e 761 /****** printLineScanData() ****************************************************
mrkeithcyr 0:0699eb71778e 762 Purpose: This function prints out the camera data to the terminal for use in
mrkeithcyr 0:0699eb71778e 763 testing and debugging.
mrkeithcyr 0:0699eb71778e 764 Parameters: Takes the most recent scan returned by the camera.
mrkeithcyr 0:0699eb71778e 765 Returns: None
mrkeithcyr 0:0699eb71778e 766 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 767 void printLineScanData(uint16_t* LineScanData)
mrkeithcyr 0:0699eb71778e 768 {
mrkeithcyr 0:0699eb71778e 769 uint32_t i = 0;
mrkeithcyr 0:0699eb71778e 770 float Val;
mrkeithcyr 0:0699eb71778e 771
mrkeithcyr 0:0699eb71778e 772 TERMINAL_PRINTF("LINE SCAN DATA:,");
mrkeithcyr 0:0699eb71778e 773
mrkeithcyr 0:0699eb71778e 774 // print one line worth of data (128) from Camera 0
mrkeithcyr 0:0699eb71778e 775 for(i=0;i<NUM_LINE_SCAN;i++)
mrkeithcyr 0:0699eb71778e 776 {
mrkeithcyr 0:0699eb71778e 777 if (1 == 1) { // use float to print
mrkeithcyr 0:0699eb71778e 778 Val = (float) LineScanData[i];
mrkeithcyr 0:0699eb71778e 779 TERMINAL_PRINTF("%9.3f",Val);
mrkeithcyr 0:0699eb71778e 780 if(i==MAX_LINE_SCAN) // when last data reached put in line return
mrkeithcyr 0:0699eb71778e 781 TERMINAL_PRINTF("\r\n");
mrkeithcyr 0:0699eb71778e 782 else
mrkeithcyr 0:0699eb71778e 783 TERMINAL_PRINTF(",");
mrkeithcyr 0:0699eb71778e 784 } else
mrkeithcyr 0:0699eb71778e 785 {
mrkeithcyr 0:0699eb71778e 786 TERMINAL_PRINTF("0x%X",LineScanData[i]);
mrkeithcyr 0:0699eb71778e 787 if(i==MAX_LINE_SCAN) // when last data reached put in line return
mrkeithcyr 0:0699eb71778e 788 TERMINAL_PRINTF("\r\n",LineScanData[i]);
mrkeithcyr 0:0699eb71778e 789 else
mrkeithcyr 0:0699eb71778e 790 TERMINAL_PRINTF(",",LineScanData[i]);
mrkeithcyr 0:0699eb71778e 791 }
mrkeithcyr 0:0699eb71778e 792 }
mrkeithcyr 0:0699eb71778e 793 }
mrkeithcyr 0:0699eb71778e 794
mrkeithcyr 0:0699eb71778e 795 /****** SteeringControl() ******************************************************
mrkeithcyr 0:0699eb71778e 796 Purpose: This function decides how much to turn the front wheels to keep the car
mrkeithcyr 0:0699eb71778e 797 on course.
mrkeithcyr 0:0699eb71778e 798 The CurrentSteerSetting variable is set here.
mrkeithcyr 0:0699eb71778e 799 Its results are printed out to the terminal if terminal output has been
mrkeithcyr 0:0699eb71778e 800 enabled.
mrkeithcyr 0:0699eb71778e 801 Parameters: None
mrkeithcyr 0:0699eb71778e 802 Returns: None
mrkeithcyr 0:0699eb71778e 803 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 804 void SteeringControl()
mrkeithcyr 0:0699eb71778e 805 {
mrkeithcyr 0:0699eb71778e 806 float ReadPot1 = 1;
mrkeithcyr 0:0699eb71778e 807
mrkeithcyr 0:0699eb71778e 808 // target to achieve for line position
mrkeithcyr 0:0699eb71778e 809 float targetPosition = (float)( (NUM_LINE_SCAN / 2) - 0.5);
mrkeithcyr 0:0699eb71778e 810
mrkeithcyr 0:0699eb71778e 811 float KP; // proportional control factor
mrkeithcyr 0:0699eb71778e 812 float KI; // integral control factor
mrkeithcyr 0:0699eb71778e 813 float KD; // derivative control factor
mrkeithcyr 0:0699eb71778e 814
mrkeithcyr 0:0699eb71778e 815 // PID terms
mrkeithcyr 0:0699eb71778e 816 float Pout;
mrkeithcyr 0:0699eb71778e 817 float Iout;
mrkeithcyr 0:0699eb71778e 818 float Dout;
mrkeithcyr 0:0699eb71778e 819
mrkeithcyr 0:0699eb71778e 820 // Calculate error
mrkeithcyr 0:0699eb71778e 821 // make error to the right positive
mrkeithcyr 0:0699eb71778e 822 // i.e. if LINE to the right-- then CurrentLinePosError > 0
mrkeithcyr 0:0699eb71778e 823 // if LINE to the left -- then CurrentLinePosError < 0
mrkeithcyr 0:0699eb71778e 824 CurrentLinePosError = CurrentLinePosition - targetPosition;
mrkeithcyr 0:0699eb71778e 825
mrkeithcyr 0:0699eb71778e 826 // Get absolute error
mrkeithcyr 0:0699eb71778e 827 if (CurrentLinePosError >= 0)
mrkeithcyr 0:0699eb71778e 828 AbsError = CurrentLinePosError;
mrkeithcyr 0:0699eb71778e 829 else
mrkeithcyr 0:0699eb71778e 830 AbsError = -1 * CurrentLinePosError;
mrkeithcyr 0:0699eb71778e 831
mrkeithcyr 0:0699eb71778e 832 // CHOOSE SET OF PID CONTROL PARAMETERS
mrkeithcyr 0:0699eb71778e 833 switch (CONTROL_METHOD)
mrkeithcyr 0:0699eb71778e 834 {
mrkeithcyr 0:0699eb71778e 835 // Pure proportional control based on
mrkeithcyr 0:0699eb71778e 836 // range of steering values vs. range of error values
mrkeithcyr 0:0699eb71778e 837 case 0:
mrkeithcyr 0:0699eb71778e 838 KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT )
mrkeithcyr 0:0699eb71778e 839 / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH );
mrkeithcyr 0:0699eb71778e 840 KD = 0;
mrkeithcyr 0:0699eb71778e 841 KI = 0;
mrkeithcyr 0:0699eb71778e 842 break;
mrkeithcyr 0:0699eb71778e 843 // Proportional control but 50% more aggressive around the bends
mrkeithcyr 0:0699eb71778e 844 case 1:
mrkeithcyr 0:0699eb71778e 845 ReadPot1 = TFC_ReadPot(1)+1; // pot range 0-2
mrkeithcyr 0:0699eb71778e 846 KP = (float) ( MAX_STEER_RIGHT - MAX_STEER_LEFT )
mrkeithcyr 0:0699eb71778e 847 / ( NUM_LINE_SCAN - (2*FILTER_ENDS) - MIN_LINE_WIDTH );
mrkeithcyr 0:0699eb71778e 848 KP = KP * ReadPot1;
mrkeithcyr 0:0699eb71778e 849 KD = 0;
mrkeithcyr 0:0699eb71778e 850 KI = 0;
mrkeithcyr 0:0699eb71778e 851 break;
mrkeithcyr 0:0699eb71778e 852 // MANUAL TUNING CASE 1 (use pot to help determine tuning parameters)
mrkeithcyr 0:0699eb71778e 853 case 2:
mrkeithcyr 0:0699eb71778e 854 KP = TUNE_KP;
mrkeithcyr 0:0699eb71778e 855 KI = TUNE_KI;
mrkeithcyr 0:0699eb71778e 856 KD = TUNE_KD;
mrkeithcyr 0:0699eb71778e 857 case 3:
mrkeithcyr 0:0699eb71778e 858 if (AbsError < ABS_ERROR_THRESH)
mrkeithcyr 0:0699eb71778e 859 {
mrkeithcyr 0:0699eb71778e 860 KP = 0.003; // when relatively straight, keep KP gain low
mrkeithcyr 0:0699eb71778e 861 }
mrkeithcyr 0:0699eb71778e 862 else
mrkeithcyr 0:0699eb71778e 863 {
mrkeithcyr 0:0699eb71778e 864 KP = 0.010; // when curve begins or off track, increase KP gain
mrkeithcyr 0:0699eb71778e 865 }
mrkeithcyr 0:0699eb71778e 866 KI = 0;
mrkeithcyr 0:0699eb71778e 867 KD = 0;
mrkeithcyr 0:0699eb71778e 868 default:
mrkeithcyr 0:0699eb71778e 869 break;
mrkeithcyr 0:0699eb71778e 870 }
mrkeithcyr 0:0699eb71778e 871
mrkeithcyr 0:0699eb71778e 872 /* Pseudocode
mrkeithcyr 0:0699eb71778e 873 previous_error = 0
mrkeithcyr 0:0699eb71778e 874 integral = 0
mrkeithcyr 0:0699eb71778e 875 start:
mrkeithcyr 0:0699eb71778e 876 error = setpoint - measured_value
mrkeithcyr 0:0699eb71778e 877 integral = integral + error*dt
mrkeithcyr 0:0699eb71778e 878 derivative = (error - previous_error)/dt
mrkeithcyr 0:0699eb71778e 879 output = Kp*error + Ki*integral + Kd*derivative
mrkeithcyr 0:0699eb71778e 880 previous_error = error
mrkeithcyr 0:0699eb71778e 881 wait(dt)
mrkeithcyr 0:0699eb71778e 882 goto start
mrkeithcyr 0:0699eb71778e 883 */
mrkeithcyr 0:0699eb71778e 884
mrkeithcyr 0:0699eb71778e 885 if (terminalOutput)
mrkeithcyr 0:0699eb71778e 886 {
mrkeithcyr 0:0699eb71778e 887 TERMINAL_PRINTF("KP = %6.4f\r\n", KP);
mrkeithcyr 0:0699eb71778e 888 TERMINAL_PRINTF("TARGET %6.3f\r\n", targetPosition);
mrkeithcyr 0:0699eb71778e 889 }
mrkeithcyr 0:0699eb71778e 890
mrkeithcyr 0:0699eb71778e 891 // Update integral of error
mrkeithcyr 0:0699eb71778e 892 // i.e. if LINE stays to the right, then SumLinePosError increases
mrkeithcyr 0:0699eb71778e 893 // i.e. if LINE stays to the left, then SumLinePosError decreases
mrkeithcyr 0:0699eb71778e 894 SumLinePosError = SumLinePosError + ( CurrentLinePosError * DT );
mrkeithcyr 0:0699eb71778e 895
mrkeithcyr 0:0699eb71778e 896 DerivError = (CurrentLinePosError - LastLinePosError) / DT;
mrkeithcyr 0:0699eb71778e 897
mrkeithcyr 0:0699eb71778e 898 if (terminalOutput) {
mrkeithcyr 0:0699eb71778e 899 TERMINAL_PRINTF("CURRENT LINE POSITION %9.3f\r\n",
mrkeithcyr 0:0699eb71778e 900 CurrentLinePosition);
mrkeithcyr 0:0699eb71778e 901 TERMINAL_PRINTF("CURRENT LINE POSITION ERROR %9.3f\r\n",
mrkeithcyr 0:0699eb71778e 902 CurrentLinePosError);
mrkeithcyr 0:0699eb71778e 903 }
mrkeithcyr 0:0699eb71778e 904
mrkeithcyr 0:0699eb71778e 905 // SECOND- calculate new servo position
mrkeithcyr 0:0699eb71778e 906
mrkeithcyr 0:0699eb71778e 907 // proportional control term
mrkeithcyr 0:0699eb71778e 908 Pout = KP * CurrentLinePosError;
mrkeithcyr 0:0699eb71778e 909
mrkeithcyr 0:0699eb71778e 910 // integral control term
mrkeithcyr 0:0699eb71778e 911 Iout = KI * SumLinePosError;
mrkeithcyr 0:0699eb71778e 912
mrkeithcyr 0:0699eb71778e 913 // Derivative control term
mrkeithcyr 0:0699eb71778e 914 Dout = KD * DerivError;
mrkeithcyr 0:0699eb71778e 915
mrkeithcyr 0:0699eb71778e 916 if (terminalOutput) {
mrkeithcyr 0:0699eb71778e 917 TERMINAL_PRINTF("KP = %6.4f\r\n", KP);
mrkeithcyr 0:0699eb71778e 918 TERMINAL_PRINTF("KI = %6.4f\r\n", KI);
mrkeithcyr 0:0699eb71778e 919 TERMINAL_PRINTF("KD = %6.4f\r\n", KD);
mrkeithcyr 0:0699eb71778e 920 TERMINAL_PRINTF("Pout = %6.4f\r\n", Pout);
mrkeithcyr 0:0699eb71778e 921 TERMINAL_PRINTF("Iout = %6.4f\r\n", Iout);
mrkeithcyr 0:0699eb71778e 922 TERMINAL_PRINTF("Dout = %6.4f\r\n", Dout);
mrkeithcyr 0:0699eb71778e 923 }
mrkeithcyr 0:0699eb71778e 924
mrkeithcyr 0:0699eb71778e 925 // add offset to steering to account for non-centered servo mounting
mrkeithcyr 0:0699eb71778e 926 // CurrentSteerSetting = Pout + Iout + Dout
mrkeithcyr 0:0699eb71778e 927 // + ((float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2);
mrkeithcyr 0:0699eb71778e 928 CurrentSteerSetting = Pout
mrkeithcyr 0:0699eb71778e 929 + ((float) (MAX_STEER_LEFT + MAX_STEER_RIGHT) / 2 );
mrkeithcyr 0:0699eb71778e 930
mrkeithcyr 0:0699eb71778e 931 // store for next cycle deriv calculation
mrkeithcyr 0:0699eb71778e 932 LastLinePosError = CurrentLinePosError;
mrkeithcyr 0:0699eb71778e 933
mrkeithcyr 0:0699eb71778e 934 // for tuning control algo only
mrkeithcyr 0:0699eb71778e 935 if (1 == 0)
mrkeithcyr 0:0699eb71778e 936 {
mrkeithcyr 0:0699eb71778e 937 TERMINAL_PRINTF("*** ******************************** \r\n");
mrkeithcyr 0:0699eb71778e 938 TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n",
mrkeithcyr 0:0699eb71778e 939 CurrentLinePosition);
mrkeithcyr 0:0699eb71778e 940 TERMINAL_PRINTF("*** ERROR %9.3f *** \r\n", CurrentLinePosError);
mrkeithcyr 0:0699eb71778e 941 TERMINAL_PRINTF("*** INTEGRAL ERROR %9.3f *** \r\n",
mrkeithcyr 0:0699eb71778e 942 SumLinePosError);
mrkeithcyr 0:0699eb71778e 943 TERMINAL_PRINTF("*** DERIVATIVE ERROR %9.3f *** \r\n", DerivError);
mrkeithcyr 0:0699eb71778e 944 TERMINAL_PRINTF("*** P STEER SETTING %9.3f *** \r\n",
mrkeithcyr 0:0699eb71778e 945 CurrentSteerSetting);
mrkeithcyr 0:0699eb71778e 946 TERMINAL_PRINTF("*** PI STEER SETTING %9.3f *** \r\n",
mrkeithcyr 0:0699eb71778e 947 (CurrentSteerSetting + Iout));
mrkeithcyr 0:0699eb71778e 948 TERMINAL_PRINTF("*** ******************************** \r\n");
mrkeithcyr 0:0699eb71778e 949 wait_ms(1000);
mrkeithcyr 0:0699eb71778e 950 }
mrkeithcyr 0:0699eb71778e 951 }
mrkeithcyr 0:0699eb71778e 952
mrkeithcyr 0:0699eb71778e 953 /****** Steer() ****************************************************************
mrkeithcyr 0:0699eb71778e 954 Purpose: Sets the steering servo output to the value necessary for the current
mrkeithcyr 0:0699eb71778e 955 steering needs. It also checks the value needed to make sure the board isn't
mrkeithcyr 0:0699eb71778e 956 trying to steer harder than the wheels can turn.
mrkeithcyr 0:0699eb71778e 957 Parameters: None
mrkeithcyr 0:0699eb71778e 958 Returns: None
mrkeithcyr 0:0699eb71778e 959 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 960 void Steer()
mrkeithcyr 0:0699eb71778e 961 {
mrkeithcyr 0:0699eb71778e 962 // make sure doesn't go beyond steering limits
mrkeithcyr 0:0699eb71778e 963 if (CurrentSteerSetting > MAX_STEER_RIGHT)
mrkeithcyr 0:0699eb71778e 964 {
mrkeithcyr 0:0699eb71778e 965 CurrentSteerSetting = MAX_STEER_RIGHT;
mrkeithcyr 0:0699eb71778e 966 }
mrkeithcyr 0:0699eb71778e 967 else if(CurrentSteerSetting < MAX_STEER_LEFT)
mrkeithcyr 0:0699eb71778e 968 {
mrkeithcyr 0:0699eb71778e 969 CurrentSteerSetting = MAX_STEER_LEFT;
mrkeithcyr 0:0699eb71778e 970 }
mrkeithcyr 0:0699eb71778e 971 if(terminalOutput)
mrkeithcyr 0:0699eb71778e 972 {
mrkeithcyr 0:0699eb71778e 973 TERMINAL_PRINTF("APPLYING SERVO SETTING %5.3f\r\n",
mrkeithcyr 0:0699eb71778e 974 CurrentSteerSetting);
mrkeithcyr 0:0699eb71778e 975 }
mrkeithcyr 0:0699eb71778e 976 TFC_SetServo(0,CurrentSteerSetting);
mrkeithcyr 0:0699eb71778e 977 }
mrkeithcyr 0:0699eb71778e 978
mrkeithcyr 0:0699eb71778e 979 /****** printDerivLineScanData() ***********************************************
mrkeithcyr 0:0699eb71778e 980 Purpose: This function prints out the line scan derivative data to the terminal
mrkeithcyr 0:0699eb71778e 981 for use in testing and debugging.
mrkeithcyr 0:0699eb71778e 982 Parameters: Takes the array of line scan data derivatives.
mrkeithcyr 0:0699eb71778e 983 Returns: None
mrkeithcyr 0:0699eb71778e 984 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 985 void printDerivLineScanData(float* derivLineScanData)
mrkeithcyr 0:0699eb71778e 986 {
mrkeithcyr 0:0699eb71778e 987 uint32_t i;
mrkeithcyr 0:0699eb71778e 988 uint32_t minCnt = 0;
mrkeithcyr 0:0699eb71778e 989 uint32_t maxCnt = 0;
mrkeithcyr 0:0699eb71778e 990
mrkeithcyr 0:0699eb71778e 991 minCnt = FILTER_ENDS;
mrkeithcyr 0:0699eb71778e 992 maxCnt = NUM_LINE_SCAN - FILTER_ENDS;
mrkeithcyr 0:0699eb71778e 993
mrkeithcyr 0:0699eb71778e 994 TERMINAL_PRINTF("DERIVATIVE DATA:,");
mrkeithcyr 0:0699eb71778e 995
mrkeithcyr 0:0699eb71778e 996 // print one line worth of data (128) from Camera 0
mrkeithcyr 0:0699eb71778e 997 for(i=minCnt;i<maxCnt;i++)
mrkeithcyr 0:0699eb71778e 998 {
mrkeithcyr 0:0699eb71778e 999 TERMINAL_PRINTF("%9.3f",derivLineScanData[i]);
mrkeithcyr 0:0699eb71778e 1000 if(i==maxCnt-1) // when last data reached put in line return
mrkeithcyr 0:0699eb71778e 1001 TERMINAL_PRINTF("\r\n",derivLineScanData[i]);
mrkeithcyr 0:0699eb71778e 1002 else
mrkeithcyr 0:0699eb71778e 1003 TERMINAL_PRINTF(", ",derivLineScanData[i]);
mrkeithcyr 0:0699eb71778e 1004 }
mrkeithcyr 0:0699eb71778e 1005 }
mrkeithcyr 0:0699eb71778e 1006
mrkeithcyr 0:0699eb71778e 1007 /****** printEdgesFound() ******************************************************
mrkeithcyr 0:0699eb71778e 1008 Purpose: This function prints the line edge data to the terminal for line use in
mrkeithcyr 0:0699eb71778e 1009 debugging.
mrkeithcyr 0:0699eb71778e 1010 Parameters: None
mrkeithcyr 0:0699eb71778e 1011 Returns: None
mrkeithcyr 0:0699eb71778e 1012 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 1013 void printEdgesFound()
mrkeithcyr 0:0699eb71778e 1014 {
mrkeithcyr 0:0699eb71778e 1015 int i;
mrkeithcyr 0:0699eb71778e 1016
mrkeithcyr 0:0699eb71778e 1017 // Check that neg edges captured ok
mrkeithcyr 0:0699eb71778e 1018 TERMINAL_PRINTF("NEGATIVE EDGES FOUND:,");
mrkeithcyr 0:0699eb71778e 1019 for(i=0;i<=numNegEdges-1;i++)
mrkeithcyr 0:0699eb71778e 1020 {
mrkeithcyr 0:0699eb71778e 1021 TERMINAL_PRINTF("%9.3f",NegEdges[i]);
mrkeithcyr 0:0699eb71778e 1022 // when last data reached put in line return
mrkeithcyr 0:0699eb71778e 1023 if(i==numNegEdges-1)
mrkeithcyr 0:0699eb71778e 1024 TERMINAL_PRINTF("\r\n");
mrkeithcyr 0:0699eb71778e 1025 else
mrkeithcyr 0:0699eb71778e 1026 TERMINAL_PRINTF(", ");
mrkeithcyr 0:0699eb71778e 1027 }
mrkeithcyr 0:0699eb71778e 1028
mrkeithcyr 0:0699eb71778e 1029 // Check that pos edges captured ok
mrkeithcyr 0:0699eb71778e 1030 TERMINAL_PRINTF("POSITIVE EDGES FOUND:,");
mrkeithcyr 0:0699eb71778e 1031 for(i=0;i<=numPosEdges-1;i++)
mrkeithcyr 0:0699eb71778e 1032 {
mrkeithcyr 0:0699eb71778e 1033 TERMINAL_PRINTF("%9.3f",PosEdges[i]);
mrkeithcyr 0:0699eb71778e 1034 // when last data reached put in line return
mrkeithcyr 0:0699eb71778e 1035 if(i==numPosEdges-1)
mrkeithcyr 0:0699eb71778e 1036 TERMINAL_PRINTF("\r\n");
mrkeithcyr 0:0699eb71778e 1037 else
mrkeithcyr 0:0699eb71778e 1038 TERMINAL_PRINTF(", ");
mrkeithcyr 0:0699eb71778e 1039 }
mrkeithcyr 0:0699eb71778e 1040 }
mrkeithcyr 0:0699eb71778e 1041
mrkeithcyr 0:0699eb71778e 1042 /****** PrintOutSomeData() *****************************************************
mrkeithcyr 0:0699eb71778e 1043 Purpose: Prints out a selection of Track mode data after image processing.
mrkeithcyr 0:0699eb71778e 1044 Parameters: None
mrkeithcyr 0:0699eb71778e 1045 Returns: None
mrkeithcyr 0:0699eb71778e 1046 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 1047 void PrintOutSomeData()
mrkeithcyr 0:0699eb71778e 1048 {
mrkeithcyr 0:0699eb71778e 1049 printLineScanData(&GrabLineScanImage0[0]);
mrkeithcyr 0:0699eb71778e 1050 printDerivLineScanData(&DerivLineScanImage0[0]);
mrkeithcyr 0:0699eb71778e 1051 printAdjustLightsData();
mrkeithcyr 0:0699eb71778e 1052 printEdgesFound();
mrkeithcyr 0:0699eb71778e 1053 }
mrkeithcyr 0:0699eb71778e 1054
mrkeithcyr 0:0699eb71778e 1055 /****** ActOnTrackStatus() *****************************************************
mrkeithcyr 0:0699eb71778e 1056 Purpose: This function decides what to do next based on the current track
mrkeithcyr 0:0699eb71778e 1057 status. Its results are printed out to the terminal if terminal output has
mrkeithcyr 0:0699eb71778e 1058 been enabled.
mrkeithcyr 0:0699eb71778e 1059 Parameters: None
mrkeithcyr 0:0699eb71778e 1060 Returns: None
mrkeithcyr 0:0699eb71778e 1061 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 1062 void ActOnTrackStatus()
mrkeithcyr 0:0699eb71778e 1063 {
mrkeithcyr 0:0699eb71778e 1064 // Decide what to do next based on current track status
mrkeithcyr 0:0699eb71778e 1065
mrkeithcyr 0:0699eb71778e 1066 // LINE FOUND!
mrkeithcyr 0:0699eb71778e 1067 if (CurrentTrackStatus == LineFound)
mrkeithcyr 0:0699eb71778e 1068 {
mrkeithcyr 0:0699eb71778e 1069 if (terminalOutput) {
mrkeithcyr 0:0699eb71778e 1070 TERMINAL_PRINTF("***************************************** \r\n");
mrkeithcyr 0:0699eb71778e 1071 TERMINAL_PRINTF("*** LINE FOUND AT POSITION %9.3f *** \r\n",
mrkeithcyr 0:0699eb71778e 1072 CurrentLinePosition);
mrkeithcyr 0:0699eb71778e 1073 TERMINAL_PRINTF("***************************************** \r\n");
mrkeithcyr 0:0699eb71778e 1074 }
mrkeithcyr 0:0699eb71778e 1075
mrkeithcyr 0:0699eb71778e 1076 // Update steering position
mrkeithcyr 0:0699eb71778e 1077 SteeringControl();
mrkeithcyr 0:0699eb71778e 1078
mrkeithcyr 0:0699eb71778e 1079 // Apply to servo
mrkeithcyr 0:0699eb71778e 1080 Steer();
mrkeithcyr 0:0699eb71778e 1081 }
mrkeithcyr 0:0699eb71778e 1082
mrkeithcyr 0:0699eb71778e 1083 // STARTING GATE FOUND
mrkeithcyr 0:0699eb71778e 1084 else if (CurrentTrackStatus == StartGateFound)
mrkeithcyr 0:0699eb71778e 1085 {
mrkeithcyr 0:0699eb71778e 1086 if (terminalOutput)
mrkeithcyr 0:0699eb71778e 1087 {
mrkeithcyr 0:0699eb71778e 1088 TERMINAL_PRINTF("***************************************** \r\n");
mrkeithcyr 0:0699eb71778e 1089 TERMINAL_PRINTF("********** STARTING GATE FOUND ********** \r\n");
mrkeithcyr 0:0699eb71778e 1090 TERMINAL_PRINTF("********** count = %d ********** \r\n",
mrkeithcyr 0:0699eb71778e 1091 StartGateFoundCount);
mrkeithcyr 0:0699eb71778e 1092 TERMINAL_PRINTF("***************************************** \r\n");
mrkeithcyr 0:0699eb71778e 1093 }
mrkeithcyr 0:0699eb71778e 1094
mrkeithcyr 0:0699eb71778e 1095 // END RACE!
mrkeithcyr 0:0699eb71778e 1096 if (startGateStop)
mrkeithcyr 0:0699eb71778e 1097 {
mrkeithcyr 0:0699eb71778e 1098 if (StartGateFoundCount > STARTGATEFOUNDMAX)
mrkeithcyr 0:0699eb71778e 1099 {
mrkeithcyr 0:0699eb71778e 1100 go = false; // STOP!!
mrkeithcyr 0:0699eb71778e 1101 }
mrkeithcyr 0:0699eb71778e 1102 }
mrkeithcyr 0:0699eb71778e 1103 }
mrkeithcyr 0:0699eb71778e 1104 }
mrkeithcyr 0:0699eb71778e 1105
mrkeithcyr 0:0699eb71778e 1106 /****** feedbackLights() *******************************************************
mrkeithcyr 0:0699eb71778e 1107 Purpose: Turns on board LED's to indicate specific status for debugging
mrkeithcyr 0:0699eb71778e 1108 purposes.
mrkeithcyr 0:0699eb71778e 1109 Parameters: None
mrkeithcyr 0:0699eb71778e 1110 Returns: None
mrkeithcyr 0:0699eb71778e 1111 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 1112 void feedbackLights()
mrkeithcyr 0:0699eb71778e 1113 {
mrkeithcyr 0:0699eb71778e 1114 switch (CurrentTrackStatus)
mrkeithcyr 0:0699eb71778e 1115 {
mrkeithcyr 0:0699eb71778e 1116 case LineFound:
mrkeithcyr 0:0699eb71778e 1117 TFC_BAT_LED0_OFF;
mrkeithcyr 0:0699eb71778e 1118 TFC_BAT_LED1_ON;
mrkeithcyr 0:0699eb71778e 1119 TFC_BAT_LED2_ON;
mrkeithcyr 0:0699eb71778e 1120 TFC_BAT_LED3_OFF;
mrkeithcyr 0:0699eb71778e 1121 break;
mrkeithcyr 0:0699eb71778e 1122 case StartGateFound:
mrkeithcyr 0:0699eb71778e 1123 TFC_BAT_LED0_ON;
mrkeithcyr 0:0699eb71778e 1124 TFC_BAT_LED1_OFF;
mrkeithcyr 0:0699eb71778e 1125 TFC_BAT_LED2_OFF;
mrkeithcyr 0:0699eb71778e 1126 TFC_BAT_LED3_ON;
mrkeithcyr 0:0699eb71778e 1127 break;
mrkeithcyr 0:0699eb71778e 1128 default:
mrkeithcyr 0:0699eb71778e 1129 TFC_BAT_LED0_OFF;
mrkeithcyr 0:0699eb71778e 1130 TFC_BAT_LED1_OFF;
mrkeithcyr 0:0699eb71778e 1131 TFC_BAT_LED2_OFF;
mrkeithcyr 0:0699eb71778e 1132 TFC_BAT_LED3_OFF;
mrkeithcyr 0:0699eb71778e 1133 }
mrkeithcyr 0:0699eb71778e 1134 }
mrkeithcyr 0:0699eb71778e 1135
mrkeithcyr 0:0699eb71778e 1136 /****** SpeedControl() *********************************************************
mrkeithcyr 0:0699eb71778e 1137 Purpose: This function determines the speed settings in the code and set on the
mrkeithcyr 0:0699eb71778e 1138 board itself. It then offers multiple ways to apply those settings to the
mrkeithcyr 0:0699eb71778e 1139 actual speed output to the wheels.
mrkeithcyr 0:0699eb71778e 1140 Its results are printed out to the terminal if terminal output has been
mrkeithcyr 0:0699eb71778e 1141 enabled.
mrkeithcyr 0:0699eb71778e 1142 Parameters: None
mrkeithcyr 0:0699eb71778e 1143 Returns: None
mrkeithcyr 0:0699eb71778e 1144 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 1145 void SpeedControl()
mrkeithcyr 0:0699eb71778e 1146 {
mrkeithcyr 0:0699eb71778e 1147 // Get max speed setting from reading pot1 and then adjust
mrkeithcyr 0:0699eb71778e 1148 float ErrLimit;
mrkeithcyr 0:0699eb71778e 1149 float LeftDriveRatio;
mrkeithcyr 0:0699eb71778e 1150 float RightDriveRatio;
mrkeithcyr 0:0699eb71778e 1151
mrkeithcyr 0:0699eb71778e 1152 // set maximum speed
mrkeithcyr 0:0699eb71778e 1153 if (doRisky)
mrkeithcyr 0:0699eb71778e 1154 MaxSpeed = LUDICROUS_SPEED * ((TFC_ReadPot(0)+1)/2.0); // faster
mrkeithcyr 0:0699eb71778e 1155 else
mrkeithcyr 0:0699eb71778e 1156 MaxSpeed = LIGHT_SPEED * ((TFC_ReadPot(0)+1)/2.0); // slower
mrkeithcyr 0:0699eb71778e 1157
mrkeithcyr 0:0699eb71778e 1158 switch (SPEED_ADJUST)
mrkeithcyr 0:0699eb71778e 1159 {
mrkeithcyr 0:0699eb71778e 1160 // SPEED ADJUST METHOD 0
mrkeithcyr 0:0699eb71778e 1161 // no speed adjust
mrkeithcyr 0:0699eb71778e 1162 case 0:
mrkeithcyr 0:0699eb71778e 1163 LeftDriveRatio = MAX_POWER;
mrkeithcyr 0:0699eb71778e 1164 RightDriveRatio = LeftDriveRatio;
mrkeithcyr 0:0699eb71778e 1165
mrkeithcyr 0:0699eb71778e 1166 // SPEED ADJUST METHOD 1
mrkeithcyr 0:0699eb71778e 1167 // High speed when error is low, low speed when error is high
mrkeithcyr 0:0699eb71778e 1168 // lower speed when more than third outside of center
mrkeithcyr 0:0699eb71778e 1169 case 1:
mrkeithcyr 0:0699eb71778e 1170 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.33;
mrkeithcyr 0:0699eb71778e 1171 if (AbsError > ErrLimit) {
mrkeithcyr 0:0699eb71778e 1172 LeftDriveRatio = MIN_POWER;
mrkeithcyr 0:0699eb71778e 1173 } else {
mrkeithcyr 0:0699eb71778e 1174 LeftDriveRatio = MAX_POWER;
mrkeithcyr 0:0699eb71778e 1175 }
mrkeithcyr 0:0699eb71778e 1176 RightDriveRatio = LeftDriveRatio;
mrkeithcyr 0:0699eb71778e 1177 break;
mrkeithcyr 0:0699eb71778e 1178
mrkeithcyr 0:0699eb71778e 1179 // SPEED ADJUST METHOD 2
mrkeithcyr 0:0699eb71778e 1180 // max/min speed adjusts proportional to absolute value of line error
mrkeithcyr 0:0699eb71778e 1181 case 2:
mrkeithcyr 0:0699eb71778e 1182 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO;
mrkeithcyr 0:0699eb71778e 1183 LeftDriveRatio =
mrkeithcyr 0:0699eb71778e 1184 MAX_POWER - ((MAX_POWER - MIN_POWER) * (AbsError / ErrLimit));
mrkeithcyr 0:0699eb71778e 1185 RightDriveRatio = LeftDriveRatio;
mrkeithcyr 0:0699eb71778e 1186 break;
mrkeithcyr 0:0699eb71778e 1187
mrkeithcyr 0:0699eb71778e 1188 // SPEED ADJUST METHOD 3
mrkeithcyr 0:0699eb71778e 1189 // wheel relative speed proportional to absolute value of line error
mrkeithcyr 0:0699eb71778e 1190 case 3:
mrkeithcyr 0:0699eb71778e 1191 // heading right, slow right wheel down
mrkeithcyr 0:0699eb71778e 1192 if (CurrentLinePosError > 0)
mrkeithcyr 0:0699eb71778e 1193 {
mrkeithcyr 0:0699eb71778e 1194 LeftDriveRatio = MAX_POWER;
mrkeithcyr 0:0699eb71778e 1195 RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER)
mrkeithcyr 0:0699eb71778e 1196 * (CurrentLinePosError * 2 / ( (float) RANGE ) );
mrkeithcyr 0:0699eb71778e 1197 }
mrkeithcyr 0:0699eb71778e 1198 // heading left, slow left wheel down
mrkeithcyr 0:0699eb71778e 1199 else if (CurrentLinePosError < 0)
mrkeithcyr 0:0699eb71778e 1200 {
mrkeithcyr 0:0699eb71778e 1201 // note sign change due to error being negative
mrkeithcyr 0:0699eb71778e 1202 LeftDriveRatio = MAX_POWER - (MIN_POWER - MAX_POWER)
mrkeithcyr 0:0699eb71778e 1203 * (CurrentLinePosError * 2 / ( (float) RANGE ) );
mrkeithcyr 0:0699eb71778e 1204 RightDriveRatio = MAX_POWER;
mrkeithcyr 0:0699eb71778e 1205 }
mrkeithcyr 0:0699eb71778e 1206 else
mrkeithcyr 0:0699eb71778e 1207 {
mrkeithcyr 0:0699eb71778e 1208 LeftDriveRatio = MAX_POWER;
mrkeithcyr 0:0699eb71778e 1209 RightDriveRatio = MAX_POWER;
mrkeithcyr 0:0699eb71778e 1210 }
mrkeithcyr 0:0699eb71778e 1211 break;
mrkeithcyr 0:0699eb71778e 1212 case 4:
mrkeithcyr 0:0699eb71778e 1213 // SPEED ADJUST METHOD 4
mrkeithcyr 0:0699eb71778e 1214 // wheel relative speed proportional to absolute value of line error
mrkeithcyr 0:0699eb71778e 1215 // only when above a certain error
mrkeithcyr 0:0699eb71778e 1216 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.1;
mrkeithcyr 0:0699eb71778e 1217
mrkeithcyr 0:0699eb71778e 1218 // right turn-- slow right wheel down a bit
mrkeithcyr 0:0699eb71778e 1219 if (CurrentLinePosError > ErrLimit)
mrkeithcyr 0:0699eb71778e 1220 {
mrkeithcyr 0:0699eb71778e 1221 LeftDriveRatio = MAX_POWER;
mrkeithcyr 0:0699eb71778e 1222 RightDriveRatio = MAX_POWER - (MAX_POWER - MIN_POWER)
mrkeithcyr 0:0699eb71778e 1223 * (CurrentLinePosError * 2 / ( (float) RANGE ) );
mrkeithcyr 0:0699eb71778e 1224 }
mrkeithcyr 0:0699eb71778e 1225
mrkeithcyr 0:0699eb71778e 1226 // left turn-- slow left wheel down a bit
mrkeithcyr 0:0699eb71778e 1227 else if(CurrentLinePosError < (-1 * ErrLimit))
mrkeithcyr 0:0699eb71778e 1228 {
mrkeithcyr 0:0699eb71778e 1229 // note sign change due to error being negative
mrkeithcyr 0:0699eb71778e 1230 LeftDriveRatio = MAX_POWER - (MIN_POWER - MAX_POWER)
mrkeithcyr 0:0699eb71778e 1231 * (CurrentLinePosError * 2 / ( (float) RANGE ) );
mrkeithcyr 0:0699eb71778e 1232 RightDriveRatio = MAX_POWER;
mrkeithcyr 0:0699eb71778e 1233 }
mrkeithcyr 0:0699eb71778e 1234
mrkeithcyr 0:0699eb71778e 1235 // when in center drive full speed
mrkeithcyr 0:0699eb71778e 1236 else
mrkeithcyr 0:0699eb71778e 1237 {
mrkeithcyr 0:0699eb71778e 1238 LeftDriveRatio = MAX_POWER;
mrkeithcyr 0:0699eb71778e 1239 RightDriveRatio = MAX_POWER;
mrkeithcyr 0:0699eb71778e 1240 }
mrkeithcyr 0:0699eb71778e 1241 break;
mrkeithcyr 0:0699eb71778e 1242
mrkeithcyr 0:0699eb71778e 1243 // SPEED ADJUST METHOD 5
mrkeithcyr 0:0699eb71778e 1244 // High speed when error is low, low speed when error is high
mrkeithcyr 0:0699eb71778e 1245 // lower speed when more than third outside of center
mrkeithcyr 0:0699eb71778e 1246 case 5:
mrkeithcyr 0:0699eb71778e 1247 ErrLimit = ((float) RANGE ) * 0.5 * ERR_RATIO * 0.2;
mrkeithcyr 0:0699eb71778e 1248 if (AbsError > ErrLimit)
mrkeithcyr 0:0699eb71778e 1249 {
mrkeithcyr 0:0699eb71778e 1250 LeftDriveRatio = MIN_POWER;
mrkeithcyr 0:0699eb71778e 1251 }
mrkeithcyr 0:0699eb71778e 1252
mrkeithcyr 0:0699eb71778e 1253 else
mrkeithcyr 0:0699eb71778e 1254 {
mrkeithcyr 0:0699eb71778e 1255 LeftDriveRatio = MAX_POWER;
mrkeithcyr 0:0699eb71778e 1256 }
mrkeithcyr 0:0699eb71778e 1257
mrkeithcyr 0:0699eb71778e 1258 RightDriveRatio = LeftDriveRatio;
mrkeithcyr 0:0699eb71778e 1259 break;
mrkeithcyr 0:0699eb71778e 1260
mrkeithcyr 0:0699eb71778e 1261 // SPEED ADJUST METHOD 6
mrkeithcyr 0:0699eb71778e 1262 // High speed when error is low, low speed when error is high
mrkeithcyr 0:0699eb71778e 1263 // lower speed when more than third outside of center
mrkeithcyr 0:0699eb71778e 1264 case 6:
mrkeithcyr 0:0699eb71778e 1265 if (AbsError > ABS_ERROR_THRESH)
mrkeithcyr 0:0699eb71778e 1266 {
mrkeithcyr 0:0699eb71778e 1267 LeftDriveRatio = MIN_POWER;
mrkeithcyr 0:0699eb71778e 1268 }
mrkeithcyr 0:0699eb71778e 1269
mrkeithcyr 0:0699eb71778e 1270 else
mrkeithcyr 0:0699eb71778e 1271 {
mrkeithcyr 0:0699eb71778e 1272 LeftDriveRatio = MAX_POWER;
mrkeithcyr 0:0699eb71778e 1273 }
mrkeithcyr 0:0699eb71778e 1274
mrkeithcyr 0:0699eb71778e 1275 RightDriveRatio = LeftDriveRatio;
mrkeithcyr 0:0699eb71778e 1276 break;
mrkeithcyr 0:0699eb71778e 1277
mrkeithcyr 0:0699eb71778e 1278 default:
mrkeithcyr 0:0699eb71778e 1279 break;
mrkeithcyr 0:0699eb71778e 1280 }
mrkeithcyr 0:0699eb71778e 1281 // TBD-- add speed adjust based on Xaccel sensor!
mrkeithcyr 0:0699eb71778e 1282
mrkeithcyr 0:0699eb71778e 1283 // currently no control mechanism as don't have speed sensor
mrkeithcyr 0:0699eb71778e 1284 CurrentLeftDriveSetting = (float) (LeftDriveRatio / 100) * MaxSpeed;
mrkeithcyr 0:0699eb71778e 1285 CurrentRightDriveSetting = (float) (RightDriveRatio / 100) * MaxSpeed;
mrkeithcyr 0:0699eb71778e 1286
mrkeithcyr 0:0699eb71778e 1287 if (terminalOutput) {
mrkeithcyr 0:0699eb71778e 1288 TERMINAL_PRINTF("Abs Error: %4.2f\r\n", AbsError);
mrkeithcyr 0:0699eb71778e 1289 TERMINAL_PRINTF("Error Limit: %4.2f\r\n", ErrLimit);
mrkeithcyr 0:0699eb71778e 1290 TERMINAL_PRINTF("MAX SPEED = %5.2f\r\n", MaxSpeed);
mrkeithcyr 0:0699eb71778e 1291 TERMINAL_PRINTF("Current Left Drive Setting: %5.2f\r\n",
mrkeithcyr 0:0699eb71778e 1292 CurrentLeftDriveSetting);
mrkeithcyr 0:0699eb71778e 1293 TERMINAL_PRINTF("Current Right Drive Setting: %5.2f\r\n",
mrkeithcyr 0:0699eb71778e 1294 CurrentRightDriveSetting);
mrkeithcyr 0:0699eb71778e 1295 }
mrkeithcyr 0:0699eb71778e 1296 }
mrkeithcyr 0:0699eb71778e 1297
mrkeithcyr 0:0699eb71778e 1298 /****** Drive() ****************************************************************
mrkeithcyr 0:0699eb71778e 1299 Purpose: This is the function that enables the motors to run. It monitors the
mrkeithcyr 0:0699eb71778e 1300 pressing of buttons A and B to start and stop the car. It also monitors for
mrkeithcyr 0:0699eb71778e 1301 emergency stop conditions and will turn off the motors to stop the car if
mrkeithcyr 0:0699eb71778e 1302 necessary.
mrkeithcyr 0:0699eb71778e 1303 Parameters: None
mrkeithcyr 0:0699eb71778e 1304 Returns: None
mrkeithcyr 0:0699eb71778e 1305 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 1306 void Drive()
mrkeithcyr 0:0699eb71778e 1307 {
mrkeithcyr 0:0699eb71778e 1308 // START!
mrkeithcyr 0:0699eb71778e 1309 // if not going, go when button A is pressed
mrkeithcyr 0:0699eb71778e 1310 if (!go)
mrkeithcyr 0:0699eb71778e 1311 {
mrkeithcyr 0:0699eb71778e 1312 if(TFC_PUSH_BUTTON_0_PRESSED)
mrkeithcyr 0:0699eb71778e 1313 {
mrkeithcyr 0:0699eb71778e 1314 go = true;
mrkeithcyr 0:0699eb71778e 1315 UnknownCount = 0;
mrkeithcyr 0:0699eb71778e 1316 StartGateFoundCount = 0;
mrkeithcyr 0:0699eb71778e 1317 startRaceTicker = TFC_Ticker[0]; // keep track of start of race
mrkeithcyr 0:0699eb71778e 1318 logDataIndex = 0; // reset log data index
mrkeithcyr 0:0699eb71778e 1319 }
mrkeithcyr 0:0699eb71778e 1320 }
mrkeithcyr 0:0699eb71778e 1321
mrkeithcyr 0:0699eb71778e 1322 // STOP!
mrkeithcyr 0:0699eb71778e 1323 // if going, stop when button B is pressed
mrkeithcyr 0:0699eb71778e 1324 if (go) {
mrkeithcyr 0:0699eb71778e 1325 if(TFC_PUSH_BUTTON_1_PRESSED) {
mrkeithcyr 0:0699eb71778e 1326 go = false;
mrkeithcyr 0:0699eb71778e 1327 StartGateFoundCount = 0;
mrkeithcyr 0:0699eb71778e 1328 }
mrkeithcyr 0:0699eb71778e 1329 }
mrkeithcyr 0:0699eb71778e 1330
mrkeithcyr 0:0699eb71778e 1331 // EMERGENCY STOP!
mrkeithcyr 0:0699eb71778e 1332 // 'kill switch' to prevent crashes off-track
mrkeithcyr 0:0699eb71778e 1333 if (killSwitch)
mrkeithcyr 0:0699eb71778e 1334 {
mrkeithcyr 0:0699eb71778e 1335 // if track not found after certain time
mrkeithcyr 0:0699eb71778e 1336 if (UnknownCount > UNKNOWN_COUNT_MAX)
mrkeithcyr 0:0699eb71778e 1337 {
mrkeithcyr 0:0699eb71778e 1338 // kill engine
mrkeithcyr 0:0699eb71778e 1339 go = false;
mrkeithcyr 0:0699eb71778e 1340 StartGateFoundCount = 0;
mrkeithcyr 0:0699eb71778e 1341 }
mrkeithcyr 0:0699eb71778e 1342 }
mrkeithcyr 0:0699eb71778e 1343
mrkeithcyr 0:0699eb71778e 1344 // stop!
mrkeithcyr 0:0699eb71778e 1345 if (!go)
mrkeithcyr 0:0699eb71778e 1346 {
mrkeithcyr 0:0699eb71778e 1347 // make sure motors are off
mrkeithcyr 0:0699eb71778e 1348 TFC_SetMotorPWM(0,0);
mrkeithcyr 0:0699eb71778e 1349 TFC_HBRIDGE_DISABLE;
mrkeithcyr 0:0699eb71778e 1350 }
mrkeithcyr 0:0699eb71778e 1351
mrkeithcyr 0:0699eb71778e 1352 // go!
mrkeithcyr 0:0699eb71778e 1353 if (go)
mrkeithcyr 0:0699eb71778e 1354 {
mrkeithcyr 0:0699eb71778e 1355 TFC_HBRIDGE_ENABLE;
mrkeithcyr 0:0699eb71778e 1356 // motor A = right, motor B = left based on way it is mounted
mrkeithcyr 0:0699eb71778e 1357 TFC_SetMotorPWM(CurrentRightDriveSetting,CurrentLeftDriveSetting);
mrkeithcyr 0:0699eb71778e 1358 }
mrkeithcyr 0:0699eb71778e 1359 }
mrkeithcyr 0:0699eb71778e 1360
mrkeithcyr 0:0699eb71778e 1361 /****** GoSlow() ***************************************************************
mrkeithcyr 0:0699eb71778e 1362 Purpose: Sets the doRisky variable to false
mrkeithcyr 0:0699eb71778e 1363 Parameters: None
mrkeithcyr 0:0699eb71778e 1364 Returns: None
mrkeithcyr 0:0699eb71778e 1365 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 1366 void GoSlow()
mrkeithcyr 0:0699eb71778e 1367 {
mrkeithcyr 0:0699eb71778e 1368 doRisky = false;
mrkeithcyr 0:0699eb71778e 1369 }
mrkeithcyr 0:0699eb71778e 1370
mrkeithcyr 0:0699eb71778e 1371 /****** GoFast() ***************************************************************
mrkeithcyr 0:0699eb71778e 1372 Purpose: Sets the doRisky variable to true
mrkeithcyr 0:0699eb71778e 1373 Parameters: None
mrkeithcyr 0:0699eb71778e 1374 Returns: None
mrkeithcyr 0:0699eb71778e 1375 *******************************************************************************/
mrkeithcyr 0:0699eb71778e 1376 void GoFast()
mrkeithcyr 0:0699eb71778e 1377 {
mrkeithcyr 0:0699eb71778e 1378 doRisky = true;
mrkeithcyr 0:0699eb71778e 1379 }