This code enables our mouse to go round the track using the two error methods.
Dependencies: Nucleo_blink_led mbed
Fork of Nucleo_blink_led by
main.cpp@6:a652deaae134, 2018-04-01 (annotated)
- Committer:
- JamieBignell
- Date:
- Sun Apr 01 18:21:50 2018 +0000
- Revision:
- 6:a652deaae134
- Parent:
- 5:dfea493f7a12
- Child:
- 7:11dd5581c763
working version
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JamieBignell | 5:dfea493f7a12 | 1 | #include <mbed.h> |
JamieBignell | 5:dfea493f7a12 | 2 | #include <funcdef.h> |
JamieBignell | 5:dfea493f7a12 | 3 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
JamieBignell | 5:dfea493f7a12 | 4 | //Initialise Global Variables |
JamieBignell | 5:dfea493f7a12 | 5 | |
JamieBignell | 5:dfea493f7a12 | 6 | int sensorValue[] = {0,0,0,0,0,0,0,0}; |
JamieBignell | 5:dfea493f7a12 | 7 | |
JamieBignell | 5:dfea493f7a12 | 8 | int sensorMinValue[] = {0,0,0,0,0,0,0,0}; |
JamieBignell | 5:dfea493f7a12 | 9 | int sensorMaxValue[] = {0,0,0,0,0,0,0,0}; |
JamieBignell | 5:dfea493f7a12 | 10 | |
JamieBignell | 5:dfea493f7a12 | 11 | int sensorMinValue1[] = {0,0,0,0,0,0,0,0}; |
JamieBignell | 5:dfea493f7a12 | 12 | int sensorMaxValue1[] = {0,0,0,0,0,0,0,0}; |
JamieBignell | 5:dfea493f7a12 | 13 | |
JamieBignell | 5:dfea493f7a12 | 14 | int sensorMinValue2[] = {0,0,0,0,0,0,0,0}; |
JamieBignell | 5:dfea493f7a12 | 15 | int sensorMaxValue2[] = {0,0,0,0,0,0,0,0}; |
JamieBignell | 5:dfea493f7a12 | 16 | |
JamieBignell | 5:dfea493f7a12 | 17 | int sensorThreshold[] = {0,0,0,0,0,0,0,0}; |
JamieBignell | 6:a652deaae134 | 18 | float sensorErrors[] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; |
JamieBignell | 5:dfea493f7a12 | 19 | |
JamieBignell | 6:a652deaae134 | 20 | float Prop=0; |
JamieBignell | 6:a652deaae134 | 21 | float Inte=0; |
JamieBignell | 6:a652deaae134 | 22 | float Dere=0; |
JamieBignell | 5:dfea493f7a12 | 23 | |
JamieBignell | 6:a652deaae134 | 24 | float errorVar=0; |
JamieBignell | 6:a652deaae134 | 25 | float previousError=0; |
JamieBignell | 6:a652deaae134 | 26 | float PIDs=0; |
JamieBignell | 6:a652deaae134 | 27 | int newPIDs=0; |
JamieBignell | 6:a652deaae134 | 28 | int accumulator=0; |
JamieBignell | 6:a652deaae134 | 29 | bool stopMotors=false; |
JamieBignell | 6:a652deaae134 | 30 | |
JamieBignell | 6:a652deaae134 | 31 | int leftMotorValTemp=0; |
JamieBignell | 6:a652deaae134 | 32 | int rightMotorValTemp=0; |
JamieBignell | 6:a652deaae134 | 33 | |
JamieBignell | 6:a652deaae134 | 34 | int initialMotorspeed = 510; |
JamieBignell | 6:a652deaae134 | 35 | Ticker ticker; |
JamieBignell | 5:dfea493f7a12 | 36 | |
JamieBignell | 5:dfea493f7a12 | 37 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
JamieBignell | 5:dfea493f7a12 | 38 | //Main Code |
JamieBignell | 0:c0c329729a7a | 39 | |
JamieBignell | 6:a652deaae134 | 40 | void tracker() { |
JamieBignell | 6:a652deaae134 | 41 | //IF THE ERROR IS EQUAL TO THE LAST ERROR, START A COUNTING VARIABLE AND ADD THIS TO THE NOMINAL MOTOR SPEED (IE WHEN ERROR OF 0), IF CASE NOT TRUE, DO NOTHING/RETURN TO NORMAL SPEED FOR CORNER. |
JamieBignell | 6:a652deaae134 | 42 | if (abs(PIDs)<=75) |
JamieBignell | 6:a652deaae134 | 43 | { |
JamieBignell | 6:a652deaae134 | 44 | accumulator++; |
JamieBignell | 6:a652deaae134 | 45 | LEDVal=1; |
JamieBignell | 6:a652deaae134 | 46 | } |
JamieBignell | 6:a652deaae134 | 47 | else |
JamieBignell | 6:a652deaae134 | 48 | { |
JamieBignell | 6:a652deaae134 | 49 | accumulator=0; |
JamieBignell | 6:a652deaae134 | 50 | LEDVal=0; |
JamieBignell | 6:a652deaae134 | 51 | } |
JamieBignell | 6:a652deaae134 | 52 | |
JamieBignell | 6:a652deaae134 | 53 | if (accumulator>=4) |
JamieBignell | 6:a652deaae134 | 54 | { |
JamieBignell | 6:a652deaae134 | 55 | initialMotorspeed = 650; |
JamieBignell | 6:a652deaae134 | 56 | } |
JamieBignell | 6:a652deaae134 | 57 | else if (accumulator>=2) |
JamieBignell | 6:a652deaae134 | 58 | { |
JamieBignell | 6:a652deaae134 | 59 | initialMotorspeed = 600; |
JamieBignell | 6:a652deaae134 | 60 | } |
JamieBignell | 6:a652deaae134 | 61 | else |
JamieBignell | 6:a652deaae134 | 62 | { |
JamieBignell | 6:a652deaae134 | 63 | initialMotorspeed = 510; |
JamieBignell | 6:a652deaae134 | 64 | } |
JamieBignell | 6:a652deaae134 | 65 | } |
JamieBignell | 6:a652deaae134 | 66 | |
JamieBignell | 3:7f44ab64e96b | 67 | int main() |
JamieBignell | 5:dfea493f7a12 | 68 | { |
JamieBignell | 6:a652deaae134 | 69 | //CONFIGURE MOTORS & TURN OFF LED |
JamieBignell | 4:f1670eec4681 | 70 | |
JamieBignell | 4:f1670eec4681 | 71 | LEDVal=0; |
JamieBignell | 5:dfea493f7a12 | 72 | leftMotorVal.period_us(1000); |
JamieBignell | 5:dfea493f7a12 | 73 | rightMotorVal.period_us(1000); |
JamieBignell | 5:dfea493f7a12 | 74 | leftMotorVal.pulsewidth_us(0); |
JamieBignell | 5:dfea493f7a12 | 75 | rightMotorVal.pulsewidth_us(0); |
JamieBignell | 4:f1670eec4681 | 76 | |
JamieBignell | 5:dfea493f7a12 | 77 | wait_ms(1000); |
JamieBignell | 4:f1670eec4681 | 78 | |
JamieBignell | 6:a652deaae134 | 79 | //TURN LED ON |
JamieBignell | 6:a652deaae134 | 80 | LEDVal=1; |
JamieBignell | 6:a652deaae134 | 81 | |
JamieBignell | 5:dfea493f7a12 | 82 | //BEGIN CALIBRATION |
JamieBignell | 5:dfea493f7a12 | 83 | CalibrateFunc(); |
JamieBignell | 6:a652deaae134 | 84 | |
JamieBignell | 5:dfea493f7a12 | 85 | //TURN OFF LED AND SET PULSES TO 2 HZ |
JamieBignell | 5:dfea493f7a12 | 86 | LEDVal=0; |
JamieBignell | 6:a652deaae134 | 87 | |
JamieBignell | 5:dfea493f7a12 | 88 | //FIND LINE |
JamieBignell | 6:a652deaae134 | 89 | findLineFunc(); |
JamieBignell | 4:f1670eec4681 | 90 | |
JamieBignell | 5:dfea493f7a12 | 91 | //ONCE FOUND, HAND OVER TO MAIN ALGORITHM |
JamieBignell | 6:a652deaae134 | 92 | ticker.attach(&tracker, 0.1); |
JamieBignell | 5:dfea493f7a12 | 93 | while(1) |
JamieBignell | 5:dfea493f7a12 | 94 | { |
JamieBignell | 5:dfea493f7a12 | 95 | senseFunc(); |
JamieBignell | 6:a652deaae134 | 96 | CountingVar=!CountingVar; |
JamieBignell | 5:dfea493f7a12 | 97 | calculateFunc(); |
JamieBignell | 5:dfea493f7a12 | 98 | driveFunc(); |
JamieBignell | 4:f1670eec4681 | 99 | } |
JamieBignell | 6:a652deaae134 | 100 | |
JamieBignell | 3:7f44ab64e96b | 101 | return 0; |
JamieBignell | 3:7f44ab64e96b | 102 | } |