302-2019 Group 2 / Mbed 2 deprecated 302CombinedCode

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@5:30cf394eca48, 2019-04-23 (annotated)

Committer:
heathcor
Date:
Tue Apr 23 18:16:53 2019 +0000
Revision:
5:30cf394eca48
Parent:
4:11559b04c4ff 
digitalout for states

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jtroper 0:0e723924ae7c 1 #include "mbed.h"
Jtroper 1:b86f099a314a 2 #include <iostream>
Jtroper 4:11559b04c4ff 3 #include <math.h>
Jtroper 4:11559b04c4ff 4 //General
Jtroper 4:11559b04c4ff 5 #define TI .001f //1kHz sample time
Jtroper 4:11559b04c4ff 6 #define LEDOFF 0.0f //LED low value
Jtroper 4:11559b04c4ff 7 #define LEDON 1.0f //LED high value
Jtroper 4:11559b04c4ff 8 #define BATTRATIO 3.9625f //Battery Input ratio
Jtroper 4:11559b04c4ff 9 #define BATTCUTOFF 1.0f //Battery Cutoff threshold
Jtroper 4:11559b04c4ff 10 #define LOOPTIME 0.75f //Time(s) between loops
Jtroper 4:11559b04c4ff 11 #define ADCRATIO 0.30303f //Ratio from port input to ADC value
Jtroper 4:11559b04c4ff 12 #define ZERO 0.0f //Zero
Jtroper 3:b17ae88bfa54 13 #ifndef M_PI
Jtroper 4:11559b04c4ff 14 #define M_PI 3.14f //Defines Pi
Jtroper 3:b17ae88bfa54 15 #endif
Jtroper 2:a17f7da1ca7c 16
Jtroper 4:11559b04c4ff 17 //Driving
Jtroper 4:11559b04c4ff 18 #define dKP 5.1491f //Proportional Gain
Jtroper 4:11559b04c4ff 19 #define dKI 119.3089f //Integral Gain
Jtroper 4:11559b04c4ff 20 #define MOTORPWM 0.00004f //PWM Frequency
heathcor 5:30cf394eca48 21 #define MOTORMAX 0.9f //Max Value of Buck Converter
Jtroper 4:11559b04c4ff 22 #define MOTORMIN 0.0f //Minimum Value of Buck Converter
Jtroper 4:11559b04c4ff 23
Jtroper 4:11559b04c4ff 24 //Steering
Jtroper 4:11559b04c4ff 25 #define SERVOPWM 0.02f //Servo Frequency
Jtroper 4:11559b04c4ff 26 #define sKP 3.5f //Proportional Gain
Jtroper 4:11559b04c4ff 27 #define sKD 0.000f //Derivative Gain was .01
Jtroper 4:11559b04c4ff 28 #define DCINTERCEPT 0.061f //Duty cycle intercept was 0.061
Jtroper 4:11559b04c4ff 29 #define DCSLOPE 0.05f //Duty cycle slope
Jtroper 4:11559b04c4ff 30 #define TOL 0.005f //Tolerance
Jtroper 4:11559b04c4ff 31 #define FULLLEFT 0.1f //PWM value of servo turning left
Jtroper 4:11559b04c4ff 32 #define NLANDMARKS 10 //Number of Landmarks
Jtroper 4:11559b04c4ff 33 #define OFFTRACKRIGHT 0.401f//Right Off Track Threshold was .40152
Jtroper 4:11559b04c4ff 34 #define OFFTRACKLEFT 0.409f //Left Off Track Threshold was .4091
Jtroper 4:11559b04c4ff 35 #define STEERINGREF -0.1f //Centerpoint reference for steering was -0.1
Jtroper 4:11559b04c4ff 36
Jtroper 4:11559b04c4ff 37
Jtroper 4:11559b04c4ff 38
Jtroper 2:a17f7da1ca7c 39 //Pins
Jtroper 4:11559b04c4ff 40 //_p - pin
Jtroper 4:11559b04c4ff 41 //AI - Analog in
Jtroper 4:11559b04c4ff 42 //PWM - PWM out
Jtroper 4:11559b04c4ff 43 //DO - Digital out
Jtroper 4:11559b04c4ff 44 //IN - Interupt in
Jtroper 4:11559b04c4ff 45 //General Pins
Jtroper 4:11559b04c4ff 46 PwmOut led_red(LED1);
Jtroper 4:11559b04c4ff 47 PwmOut led_blue(LED3);
Jtroper 4:11559b04c4ff 48 PwmOut led_green(LED2);
Jtroper 3:b17ae88bfa54 49
Jtroper 4:11559b04c4ff 50 //Motor Pins
Jtroper 4:11559b04c4ff 51 AnalogIn AI_pSpeedReference(A0);
Jtroper 4:11559b04c4ff 52 AnalogIn AI_pTacho(A1);
Jtroper 4:11559b04c4ff 53 PwmOut PWM_pMotorControl(PTD0);
Jtroper 4:11559b04c4ff 54 DigitalOut DO_pBreakSignal(PTC9);
Jtroper 4:11559b04c4ff 55 AnalogIn AI_pBattMon(A4);
Jtroper 4:11559b04c4ff 56
Jtroper 4:11559b04c4ff 57 //Steering Pins
Jtroper 4:11559b04c4ff 58 AnalogIn AI_pLeftSensors(A2);
Jtroper 4:11559b04c4ff 59 AnalogIn AI_pRightSensors(A3);
Jtroper 4:11559b04c4ff 60 PwmOut PWM_pServo(PTE20);
Jtroper 3:b17ae88bfa54 61
Jtroper 4:11559b04c4ff 62 //Binary LED
Jtroper 4:11559b04c4ff 63 DigitalOut DO_pFirstBinaryLED(A5);
Jtroper 4:11559b04c4ff 64 DigitalOut DO_pSecondBinaryLED(PTE21);
Jtroper 4:11559b04c4ff 65 DigitalOut DO_pThirdBinaryLED(PTD2);
Jtroper 4:11559b04c4ff 66 DigitalOut DO_pFourthBinaryLED(PTE31);
Jtroper 2:a17f7da1ca7c 67
Jtroper 4:11559b04c4ff 68 //Hardware Interupts
Jtroper 2:a17f7da1ca7c 69 Ticker Update;
heathcor 5:30cf394eca48 70 DigitalIn IN_pRunButton(PTA5);
heathcor 5:30cf394eca48 71 DigitalIn IN_pStopButton(PTA4);
Jtroper 4:11559b04c4ff 72 InterruptIn IN_pBumperStop(PTD3);
Jtroper 4:11559b04c4ff 73
Jtroper 4:11559b04c4ff 74 InterruptIn IN_pArrowheadLeft(PTD5);
Jtroper 4:11559b04c4ff 75 InterruptIn IN_pArrowheadRight(PTA13);
Jtroper 2:a17f7da1ca7c 76
Jtroper 4:11559b04c4ff 77
Jtroper 4:11559b04c4ff 78 //Data Value
Jtroper 4:11559b04c4ff 79 //_f - Float
Jtroper 4:11559b04c4ff 80 //_n - Integer
Jtroper 4:11559b04c4ff 81 //_b - Boolean
Jtroper 4:11559b04c4ff 82 float _fErrorArea;
Jtroper 4:11559b04c4ff 83 float _fError;
Jtroper 4:11559b04c4ff 84 float _fControllerOutput;
Jtroper 4:11559b04c4ff 85 float _fMotorReference;
Jtroper 4:11559b04c4ff 86 float _fFeedbackPrev;
Jtroper 4:11559b04c4ff 87 int _nCounter;
Jtroper 4:11559b04c4ff 88 bool _bArrowHitRight;
Jtroper 4:11559b04c4ff 89 bool _bArrowHitLeft;
Jtroper 4:11559b04c4ff 90
Jtroper 3:b17ae88bfa54 91
Jtroper 3:b17ae88bfa54 92 //States
Jtroper 4:11559b04c4ff 93 //_o - object
Jtroper 3:b17ae88bfa54 94 enum States {RUN, WAIT, STOP, EMPTY};
Jtroper 4:11559b04c4ff 95 States _oNextState;
Jtroper 4:11559b04c4ff 96 States _oCurrentState;
Jtroper 2:a17f7da1ca7c 97
Jtroper 4:11559b04c4ff 98 //General Methods
Jtroper 2:a17f7da1ca7c 99 void Init();
Jtroper 2:a17f7da1ca7c 100 void Loop();
Jtroper 2:a17f7da1ca7c 101 void UpdateMethod();
Jtroper 4:11559b04c4ff 102 void UpdateLowBatteryStop();
Jtroper 4:11559b04c4ff 103 void OnboardLEDColor(float, float, float);
Jtroper 4:11559b04c4ff 104 void RunButtonMethod();
Jtroper 4:11559b04c4ff 105 void StopButtonMethod();
Jtroper 4:11559b04c4ff 106 void BumperStopMethod();
Jtroper 4:11559b04c4ff 107
Jtroper 4:11559b04c4ff 108 //Motor Methods
Jtroper 3:b17ae88bfa54 109 void UpdateDriveControl();
Jtroper 3:b17ae88bfa54 110 void UpdateSteeringControl();
Jtroper 2:a17f7da1ca7c 111 void SetMotor(float);
Jtroper 3:b17ae88bfa54 112 void BreaksOn();
Jtroper 3:b17ae88bfa54 113 void BreaksOff();
Jtroper 3:b17ae88bfa54 114
Jtroper 4:11559b04c4ff 115 //Steering Methods
Jtroper 4:11559b04c4ff 116 void UpdateLeft();
Jtroper 4:11559b04c4ff 117 void UpdateRight();
Jtroper 4:11559b04c4ff 118 void UpdateBinaryCounter();
Jtroper 4:11559b04c4ff 119
Jtroper 4:11559b04c4ff 120
Jtroper 3:b17ae88bfa54 121 //State Methods
Jtroper 4:11559b04c4ff 122 void TriggerSwitch();
Jtroper 4:11559b04c4ff 123 void StopMethod();
Jtroper 4:11559b04c4ff 124 void WaitMethod();
Jtroper 4:11559b04c4ff 125 void RunMethod();
Jtroper 0:0e723924ae7c 126
Jtroper 0:0e723924ae7c 127 int main() {
Jtroper 4:11559b04c4ff 128 cout << "\rCarStart" << endl;
Jtroper 2:a17f7da1ca7c 129 Init();
heathcor 5:30cf394eca48 130 while(1) {wait(LOOPTIME); Loop();
heathcor 5:30cf394eca48 131 if(IN_pStopButton.read()>=.9)
heathcor 5:30cf394eca48 132 StopMethod();
heathcor 5:30cf394eca48 133 if(IN_pRunButton.read()>=.9)
heathcor 5:30cf394eca48 134 TriggerSwitch();
heathcor 5:30cf394eca48 135
heathcor 5:30cf394eca48 136 }
heathcor 5:30cf394eca48 137 //int r = counter; for(int b = 3; b <= 0; b--){ if(r > pow(2,b+1)){ LED[b] = LEDON; r = r - pow(2,b+1);}
heathcor 5:30cf394eca48 138
Jtroper 2:a17f7da1ca7c 139 }
Jtroper 2:a17f7da1ca7c 140
Jtroper 2:a17f7da1ca7c 141 void Init() //Initializer
Jtroper 2:a17f7da1ca7c 142 {
Jtroper 4:11559b04c4ff 143 //Variable Init
Jtroper 4:11559b04c4ff 144 OnboardLEDColor(1.0f,1.0f,1.0f);
Jtroper 4:11559b04c4ff 145 _fError = ZERO;
Jtroper 4:11559b04c4ff 146 _fErrorArea = ZERO;
Jtroper 4:11559b04c4ff 147 _fMotorReference = ZERO;
Jtroper 4:11559b04c4ff 148 _fFeedbackPrev = ZERO;
Jtroper 4:11559b04c4ff 149 _nCounter = ZERO;
Jtroper 4:11559b04c4ff 150 _oNextState = EMPTY;
Jtroper 4:11559b04c4ff 151 _oCurrentState = EMPTY;
Jtroper 4:11559b04c4ff 152 _bArrowHitRight = false;
Jtroper 4:11559b04c4ff 153 _bArrowHitLeft = false;
Jtroper 4:11559b04c4ff 154
Jtroper 4:11559b04c4ff 155 //Motor Init
Jtroper 4:11559b04c4ff 156 PWM_pMotorControl.period(MOTORPWM);
Jtroper 4:11559b04c4ff 157 SetMotor(MOTORMIN); //Turns off the motor
Jtroper 4:11559b04c4ff 158 StopMethod();
Jtroper 4:11559b04c4ff 159 TriggerSwitch();
Jtroper 4:11559b04c4ff 160 BreaksOn();
Jtroper 4:11559b04c4ff 161
Jtroper 4:11559b04c4ff 162 //Steering Init
Jtroper 4:11559b04c4ff 163 PWM_pServo.period(SERVOPWM);
Jtroper 4:11559b04c4ff 164
Jtroper 4:11559b04c4ff 165 //Interupt Init
heathcor 5:30cf394eca48 166 //IN_pRunButton.rise(&TriggerSwitch);
heathcor 5:30cf394eca48 167 //IN_pStopButton.rise(&StopMethod);
Jtroper 4:11559b04c4ff 168 IN_pBumperStop.rise(&BumperStopMethod);
Jtroper 4:11559b04c4ff 169 IN_pArrowheadLeft.rise(&UpdateLeft);
Jtroper 4:11559b04c4ff 170 IN_pArrowheadRight.rise(&UpdateRight);
Jtroper 2:a17f7da1ca7c 171 Update.attach(&UpdateMethod, TI);
Jtroper 2:a17f7da1ca7c 172 }
Jtroper 2:a17f7da1ca7c 173
Jtroper 2:a17f7da1ca7c 174 void Loop() //Repeated Loop
Jtroper 2:a17f7da1ca7c 175 {
Jtroper 4:11559b04c4ff 176 _bArrowHitRight = false;
Jtroper 4:11559b04c4ff 177 _bArrowHitLeft = false;
Jtroper 4:11559b04c4ff 178 UpdateLowBatteryStop();
Jtroper 0:0e723924ae7c 179 }
Jtroper 2:a17f7da1ca7c 180
Jtroper 2:a17f7da1ca7c 181 void UpdateMethod() //
Jtroper 2:a17f7da1ca7c 182 {
Jtroper 3:b17ae88bfa54 183 UpdateDriveControl();
Jtroper 3:b17ae88bfa54 184 UpdateSteeringControl();
Jtroper 2:a17f7da1ca7c 185 }
Jtroper 2:a17f7da1ca7c 186
Jtroper 3:b17ae88bfa54 187 void UpdateDriveControl()
Jtroper 2:a17f7da1ca7c 188 {
Jtroper 4:11559b04c4ff 189 if(_oCurrentState == RUN)
Jtroper 3:b17ae88bfa54 190 {
heathcor 5:30cf394eca48 191 _fMotorReference = 0.85;//AI_pSpeedReference.read();
Jtroper 4:11559b04c4ff 192 _fError = _fMotorReference - AI_pTacho.read();
Jtroper 4:11559b04c4ff 193 _fErrorArea += TI*_fError;
Jtroper 4:11559b04c4ff 194 _fControllerOutput = dKP*_fError+ dKI*_fErrorArea;
Jtroper 3:b17ae88bfa54 195 }
Jtroper 4:11559b04c4ff 196 SetMotor(_fControllerOutput);
Jtroper 2:a17f7da1ca7c 197 }
Jtroper 2:a17f7da1ca7c 198
Jtroper 3:b17ae88bfa54 199 void UpdateSteeringControl(void)
Jtroper 3:b17ae88bfa54 200 {
Jtroper 4:11559b04c4ff 201 if(_oCurrentState != STOP){
Jtroper 3:b17ae88bfa54 202 //float feedbackPrev = 0;
Jtroper 4:11559b04c4ff 203 float feedback = AI_pLeftSensors.read() - AI_pRightSensors.read();
Jtroper 4:11559b04c4ff 204 float reference = STEERINGREF;
Jtroper 3:b17ae88bfa54 205 float err = reference - feedback;
Jtroper 4:11559b04c4ff 206 float feedbackChange = (/*0.0f*/feedback - _fFeedbackPrev)/TI;
Jtroper 3:b17ae88bfa54 207 float controllerOut = sKP*err + sKD*feedbackChange;
Jtroper 3:b17ae88bfa54 208 float controllerOutNorm = (controllerOut + M_PI/2.0f)/M_PI;
Jtroper 3:b17ae88bfa54 209 float dutyCycle = DCSLOPE*controllerOutNorm + DCINTERCEPT;
Jtroper 4:11559b04c4ff 210 if (abs(dutyCycle-PWM_pServo.read()) > TOL)
Jtroper 4:11559b04c4ff 211 PWM_pServo.write(dutyCycle);
Jtroper 4:11559b04c4ff 212 _fFeedbackPrev = feedback;
Jtroper 4:11559b04c4ff 213 if (AI_pLeftSensors.read() < OFFTRACKLEFT && AI_pRightSensors.read() < OFFTRACKRIGHT){
Jtroper 4:11559b04c4ff 214 StopMethod();
Jtroper 4:11559b04c4ff 215 }
Jtroper 4:11559b04c4ff 216 }
Jtroper 3:b17ae88bfa54 217
Jtroper 3:b17ae88bfa54 218 }
Jtroper 4:11559b04c4ff 219
Jtroper 4:11559b04c4ff 220 void UpdateLowBatteryStop()
Jtroper 4:11559b04c4ff 221 {
Jtroper 4:11559b04c4ff 222 float fVal = ZERO; // variable for the A/D value
Jtroper 4:11559b04c4ff 223 float fPinVoltage = ZERO; // variable to hold the calculated voltage
Jtroper 4:11559b04c4ff 224 float fBatteryVoltage = ZERO; //variable for battery voltage
Jtroper 4:11559b04c4ff 225 fVal = AI_pBattMon.read(); // read the voltage on the divider
Jtroper 4:11559b04c4ff 226
Jtroper 4:11559b04c4ff 227 fPinVoltage = fVal * ADCRATIO; // Calculate the voltage on the A/D pin
Jtroper 4:11559b04c4ff 228
Jtroper 4:11559b04c4ff 229 fBatteryVoltage = fPinVoltage * BATTRATIO; // Use the ratio calculated for the voltage divider
Jtroper 4:11559b04c4ff 230 // to calculate the battery voltage
Jtroper 4:11559b04c4ff 231 if(fBatteryVoltage < BATTCUTOFF){
Jtroper 4:11559b04c4ff 232 cout << "\n\rBattery Voltage is too low. Stop Method " << endl;
Jtroper 4:11559b04c4ff 233 StopMethod();
Jtroper 4:11559b04c4ff 234 }
Jtroper 4:11559b04c4ff 235 else{
Jtroper 4:11559b04c4ff 236 cout << "\n\rBattery Voltage is: " << fBatteryVoltage << endl;
Jtroper 4:11559b04c4ff 237 }
Jtroper 4:11559b04c4ff 238
Jtroper 4:11559b04c4ff 239 }
Jtroper 4:11559b04c4ff 240
Jtroper 2:a17f7da1ca7c 241 void SetMotor(float speed)
Jtroper 2:a17f7da1ca7c 242 {
Jtroper 4:11559b04c4ff 243 float invD = ZERO;
Jtroper 4:11559b04c4ff 244
Jtroper 4:11559b04c4ff 245 if(speed < MOTORMIN) //Speed is above floor
Jtroper 4:11559b04c4ff 246 invD = MOTORMIN;
Jtroper 4:11559b04c4ff 247 else if(speed> MOTORMAX) //Caps the speed at a max value
Jtroper 4:11559b04c4ff 248 invD = MOTORMAX;
Jtroper 2:a17f7da1ca7c 249 else
Jtroper 2:a17f7da1ca7c 250 invD = speed;
Jtroper 2:a17f7da1ca7c 251
Jtroper 4:11559b04c4ff 252 PWM_pMotorControl.write(invD);
Jtroper 3:b17ae88bfa54 253 }
Jtroper 3:b17ae88bfa54 254
Jtroper 3:b17ae88bfa54 255 void BreaksOn()
Jtroper 3:b17ae88bfa54 256 {
Jtroper 4:11559b04c4ff 257 SetMotor(MOTORMIN); //Sets speed to Zero
Jtroper 4:11559b04c4ff 258 _fErrorArea = ZERO; //Resets Error area to zero
Jtroper 4:11559b04c4ff 259 DO_pBreakSignal = 1; //Turns on breaks
Jtroper 4:11559b04c4ff 260 PWM_pServo= FULLLEFT; //Turns servo to left
Jtroper 3:b17ae88bfa54 261 }
Jtroper 3:b17ae88bfa54 262
Jtroper 3:b17ae88bfa54 263 void BreaksOff()
Jtroper 3:b17ae88bfa54 264 {
Jtroper 4:11559b04c4ff 265 SetMotor(MOTORMIN); //Sets speed to Zero
Jtroper 4:11559b04c4ff 266 _fErrorArea = ZERO; //Resets Error area to zero
Jtroper 4:11559b04c4ff 267 DO_pBreakSignal = ZERO; //Turns off brakes
Jtroper 4:11559b04c4ff 268 }
Jtroper 4:11559b04c4ff 269
Jtroper 4:11559b04c4ff 270 void WaitMethod()
Jtroper 4:11559b04c4ff 271 {
Jtroper 4:11559b04c4ff 272 _oCurrentState = WAIT;
Jtroper 4:11559b04c4ff 273 BreaksOn();
Jtroper 4:11559b04c4ff 274 OnboardLEDColor(.125f, 1.0f, 0.125f); //Sets to Yellow
Jtroper 4:11559b04c4ff 275 _oNextState = RUN;
Jtroper 4:11559b04c4ff 276 }
Jtroper 4:11559b04c4ff 277
Jtroper 4:11559b04c4ff 278 void RunMethod()
Jtroper 4:11559b04c4ff 279 {
Jtroper 4:11559b04c4ff 280 _oCurrentState = RUN;
Jtroper 4:11559b04c4ff 281 BreaksOff();
Jtroper 4:11559b04c4ff 282 OnboardLEDColor(1.0f, 1.0f, 0.25f); //Sets to Green
Jtroper 4:11559b04c4ff 283 _oNextState = WAIT;
Jtroper 4:11559b04c4ff 284 }
Jtroper 4:11559b04c4ff 285
Jtroper 4:11559b04c4ff 286 void StopMethod()
Jtroper 4:11559b04c4ff 287 {
Jtroper 4:11559b04c4ff 288 _oCurrentState = STOP;
Jtroper 4:11559b04c4ff 289 BreaksOn();
Jtroper 4:11559b04c4ff 290 OnboardLEDColor(0.25f, 1.0f, 1.0f); //Sets to Red
Jtroper 4:11559b04c4ff 291 _oNextState = WAIT;
Jtroper 4:11559b04c4ff 292 }
Jtroper 4:11559b04c4ff 293
Jtroper 4:11559b04c4ff 294 void BumperStopMethod()
Jtroper 4:11559b04c4ff 295 {
Jtroper 4:11559b04c4ff 296 _oCurrentState = STOP;
Jtroper 4:11559b04c4ff 297 BreaksOn();
Jtroper 4:11559b04c4ff 298 OnboardLEDColor(0.25f, 1.0f, 1.0f); //Sets to Red
Jtroper 4:11559b04c4ff 299 _oNextState = WAIT;
Jtroper 4:11559b04c4ff 300 }
Jtroper 4:11559b04c4ff 301
Jtroper 4:11559b04c4ff 302 void LowBatteryStopMethod()
Jtroper 4:11559b04c4ff 303 {
Jtroper 4:11559b04c4ff 304 _oCurrentState = STOP;
Jtroper 4:11559b04c4ff 305 BreaksOn();
Jtroper 4:11559b04c4ff 306 OnboardLEDColor(0.25f, 1.0f, 1.0f); //Sets to Red
Jtroper 4:11559b04c4ff 307 _oNextState = WAIT;
Jtroper 4:11559b04c4ff 308 }
Jtroper 4:11559b04c4ff 309
Jtroper 4:11559b04c4ff 310 void TriggerSwitch()
Jtroper 4:11559b04c4ff 311 {
Jtroper 4:11559b04c4ff 312 switch(_oNextState){
Jtroper 4:11559b04c4ff 313 case STOP:
Jtroper 4:11559b04c4ff 314 StopMethod();
Jtroper 4:11559b04c4ff 315 break;
Jtroper 4:11559b04c4ff 316
Jtroper 4:11559b04c4ff 317 case WAIT:
Jtroper 4:11559b04c4ff 318 WaitMethod();
Jtroper 4:11559b04c4ff 319 break;
Jtroper 4:11559b04c4ff 320
Jtroper 4:11559b04c4ff 321 case RUN:
Jtroper 4:11559b04c4ff 322 RunMethod();
Jtroper 4:11559b04c4ff 323 break;
Jtroper 4:11559b04c4ff 324
Jtroper 4:11559b04c4ff 325 case EMPTY:
Jtroper 4:11559b04c4ff 326 OnboardLEDColor(0.25f, 0.25f, 0.25f); //Sets to White
Jtroper 4:11559b04c4ff 327 _oNextState = WAIT;
Jtroper 4:11559b04c4ff 328 break;
Jtroper 4:11559b04c4ff 329 }
Jtroper 4:11559b04c4ff 330 }
Jtroper 4:11559b04c4ff 331
Jtroper 4:11559b04c4ff 332 void UpdateLeft(void)
Jtroper 4:11559b04c4ff 333 {
Jtroper 4:11559b04c4ff 334
Jtroper 4:11559b04c4ff 335 if(_nCounter >= NLANDMARKS) {
Jtroper 4:11559b04c4ff 336 _nCounter = 0; //Resets Counter when it reaches the max
Jtroper 4:11559b04c4ff 337 } else {
Jtroper 4:11559b04c4ff 338 _nCounter += 1;
Jtroper 4:11559b04c4ff 339 }
Jtroper 4:11559b04c4ff 340 if(_bArrowHitRight)
Jtroper 4:11559b04c4ff 341 {_nCounter = 0;_bArrowHitRight=!_bArrowHitRight;} //Resets Counter on double hit
Jtroper 4:11559b04c4ff 342
Jtroper 4:11559b04c4ff 343 cout << "\n\rcounter = " << _nCounter << endl;
Jtroper 4:11559b04c4ff 344 cout << "\n\rLEFT" << endl;
Jtroper 4:11559b04c4ff 345
Jtroper 4:11559b04c4ff 346 UpdateBinaryCounter();
Jtroper 4:11559b04c4ff 347 _bArrowHitLeft = true;
Jtroper 4:11559b04c4ff 348 }
Jtroper 4:11559b04c4ff 349
Jtroper 4:11559b04c4ff 350 void UpdateRight(void)
Jtroper 4:11559b04c4ff 351 {
Jtroper 4:11559b04c4ff 352
Jtroper 4:11559b04c4ff 353 if(_nCounter >= NLANDMARKS) {
Jtroper 4:11559b04c4ff 354 _nCounter = 0; //Resets Counter when it reaches the max
Jtroper 4:11559b04c4ff 355 } else {
Jtroper 4:11559b04c4ff 356 _nCounter += 1;
Jtroper 4:11559b04c4ff 357 }
Jtroper 4:11559b04c4ff 358 if(_bArrowHitLeft)
Jtroper 4:11559b04c4ff 359 {_nCounter = 0;_bArrowHitLeft=!_bArrowHitLeft;} //Resets Counter on double hit
Jtroper 4:11559b04c4ff 360
Jtroper 4:11559b04c4ff 361 cout << "\n\rcounter = " << _nCounter << endl;
Jtroper 4:11559b04c4ff 362 cout << "\n\rRIGHT" << endl;
Jtroper 4:11559b04c4ff 363
Jtroper 4:11559b04c4ff 364 UpdateBinaryCounter();
Jtroper 4:11559b04c4ff 365 _bArrowHitRight = true;
Jtroper 3:b17ae88bfa54 366 }
Jtroper 3:b17ae88bfa54 367
Jtroper 4:11559b04c4ff 368 void UpdateBinaryCounter()
Jtroper 3:b17ae88bfa54 369 {
Jtroper 4:11559b04c4ff 370 switch(_nCounter){
Jtroper 4:11559b04c4ff 371 case 0:
Jtroper 4:11559b04c4ff 372 DO_pFirstBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 373 DO_pSecondBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 374 DO_pThirdBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 375 DO_pFourthBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 376 break;
Jtroper 4:11559b04c4ff 377 case 1:
Jtroper 4:11559b04c4ff 378 DO_pFirstBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 379 DO_pSecondBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 380 DO_pThirdBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 381 DO_pFourthBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 382 break;
Jtroper 4:11559b04c4ff 383 case 2:
Jtroper 4:11559b04c4ff 384 DO_pFirstBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 385 DO_pSecondBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 386 DO_pThirdBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 387 DO_pFourthBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 388 break;
Jtroper 4:11559b04c4ff 389 case 3:
Jtroper 4:11559b04c4ff 390 DO_pFirstBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 391 DO_pSecondBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 392 DO_pThirdBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 393 DO_pFourthBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 394 break;
Jtroper 4:11559b04c4ff 395 case 4:
Jtroper 4:11559b04c4ff 396 DO_pFirstBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 397 DO_pSecondBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 398 DO_pThirdBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 399 DO_pFourthBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 400 break;
Jtroper 4:11559b04c4ff 401 case 5:
Jtroper 4:11559b04c4ff 402 DO_pFirstBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 403 DO_pSecondBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 404 DO_pThirdBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 405 DO_pFourthBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 406 break;
Jtroper 4:11559b04c4ff 407 case 6:
Jtroper 4:11559b04c4ff 408 DO_pFirstBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 409 DO_pSecondBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 410 DO_pThirdBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 411 DO_pFourthBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 412 break;
Jtroper 4:11559b04c4ff 413 case 7:
Jtroper 4:11559b04c4ff 414 DO_pFirstBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 415 DO_pSecondBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 416 DO_pThirdBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 417 DO_pFourthBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 418 break;
Jtroper 4:11559b04c4ff 419 case 8:
Jtroper 4:11559b04c4ff 420 DO_pFirstBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 421 DO_pSecondBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 422 DO_pThirdBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 423 DO_pFourthBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 424 break;
Jtroper 4:11559b04c4ff 425 case 9:
Jtroper 4:11559b04c4ff 426 DO_pFirstBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 427 DO_pSecondBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 428 DO_pThirdBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 429 DO_pFourthBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 430 break;
Jtroper 4:11559b04c4ff 431 case 10:
Jtroper 4:11559b04c4ff 432 DO_pFirstBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 433 DO_pSecondBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 434 DO_pThirdBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 435 DO_pFourthBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 436 break;
Jtroper 4:11559b04c4ff 437 case 11:
Jtroper 4:11559b04c4ff 438 DO_pFirstBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 439 DO_pSecondBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 440 DO_pThirdBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 441 DO_pFourthBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 442 break;
Jtroper 4:11559b04c4ff 443 case 12:
Jtroper 4:11559b04c4ff 444 DO_pFirstBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 445 DO_pSecondBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 446 DO_pThirdBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 447 DO_pFourthBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 448 break;
Jtroper 4:11559b04c4ff 449 case 13:
Jtroper 4:11559b04c4ff 450 DO_pFirstBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 451 DO_pSecondBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 452 DO_pThirdBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 453 DO_pFourthBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 454 break;
Jtroper 4:11559b04c4ff 455 case 14:
Jtroper 4:11559b04c4ff 456 DO_pFirstBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 457 DO_pSecondBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 458 DO_pThirdBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 459 DO_pFourthBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 460 break;
Jtroper 4:11559b04c4ff 461 case 15:
Jtroper 4:11559b04c4ff 462 DO_pFirstBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 463 DO_pSecondBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 464 DO_pThirdBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 465 DO_pFourthBinaryLED = LEDON;
Jtroper 4:11559b04c4ff 466 break;
Jtroper 4:11559b04c4ff 467 default:
Jtroper 4:11559b04c4ff 468 DO_pFirstBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 469 DO_pSecondBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 470 DO_pThirdBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 471 DO_pFourthBinaryLED = LEDOFF;
Jtroper 4:11559b04c4ff 472 break;
Jtroper 4:11559b04c4ff 473 }
Jtroper 3:b17ae88bfa54 474 }
Jtroper 4:11559b04c4ff 475
Jtroper 4:11559b04c4ff 476 void OnboardLEDColor(float fRed= 1.0f, float fGreen = 1.0f, float fBlue = 1.0f)
Jtroper 3:b17ae88bfa54 477 {
Jtroper 4:11559b04c4ff 478 led_red = fRed;
Jtroper 4:11559b04c4ff 479 led_blue = fGreen;
Jtroper 4:11559b04c4ff 480 led_green = fBlue;
Jtroper 2:a17f7da1ca7c 481 }