Railway Challenge
/
challenge-ChaiUpdated-AutostopTest-30Throttle
This is a test from 30%Throttle to stop, using full service brake
main.cpp@29:2ed4d9c309fc, 2022-06-15 (annotated)
- Committer:
- cdevarakonda
- Date:
- Wed Jun 15 13:38:05 2022 +0000
- Revision:
- 29:2ed4d9c309fc
- Parent:
- 28:1086791972d0
- Child:
- 30:c65bf90e8f47
added brake control redundancy
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rwcjoliver | 0:4788e1df7b55 | 1 | #include <mbed.h> |
rwcjoliver | 0:4788e1df7b55 | 2 | #include "millis.h" |
cdevarakonda | 28:1086791972d0 | 3 | #include "brakes.h" |
rwcjoliver | 0:4788e1df7b55 | 4 | #include "definitions.h" |
rwcjoliver | 0:4788e1df7b55 | 5 | #include "remoteControl.h" |
rwcjoliver | 0:4788e1df7b55 | 6 | #include "dashboard.h" |
rwcjoliver | 0:4788e1df7b55 | 7 | #include "rtc.h" |
rwcjoliver | 0:4788e1df7b55 | 8 | #include "motor.h" |
rwcjoliver | 0:4788e1df7b55 | 9 | #include "challenge.h" |
jamesmcildowietfl | 3:32e951e05a5b | 10 | #include <sstream> |
jamesmcildowietfl | 3:32e951e05a5b | 11 | using std::string; |
rwcjoliver | 0:4788e1df7b55 | 12 | |
rwcjoliver | 0:4788e1df7b55 | 13 | // SET UP REMOTE CONTROL COMMS |
rwcjoliver | 0:4788e1df7b55 | 14 | SPI remoteControl(PE_14, PE_13, PE_12); // (SPI_MOSI, SPI_MISO, SPI_SCK) |
rwcjoliver | 0:4788e1df7b55 | 15 | DigitalOut remoteControlCS(PE_11); // (SPI_SS) |
rwcjoliver | 0:4788e1df7b55 | 16 | |
rwcjoliver | 0:4788e1df7b55 | 17 | // CREATE OBJECTS |
rwcjoliver | 0:4788e1df7b55 | 18 | Remote remote(remoteControl, remoteControlCS); |
rwcjoliver | 0:4788e1df7b55 | 19 | Dashboard dashboard(hallSensor); |
rwcjoliver | 0:4788e1df7b55 | 20 | RoundTrainCircuit rtc(rtc_1, rtc_2, rtc_3, rtc_4, rtc_5, rtc_6, rtc_7, rtc_override); |
rwcjoliver | 0:4788e1df7b55 | 21 | Motor motor1(motorAccelerator, motorBrake, keySwitchM1, directionFwd, directionRev, footswitchM1, seatM1, inchFwdM1, speedLimit2M1, speedLimit3M1); |
rwcjoliver | 0:4788e1df7b55 | 22 | ChallengeMode challenge(autoStopTrigger, dashboard, remote, motor1); |
cdevarakonda | 28:1086791972d0 | 23 | Brakes brakes1(); |
rwcjoliver | 0:4788e1df7b55 | 24 | |
rwcjoliver | 0:4788e1df7b55 | 25 | int driveMode = 2; // Drive mode - fwd(0), rev(1), park(2) |
rwcjoliver | 0:4788e1df7b55 | 26 | bool emergencyStopActive = false; |
rwcjoliver | 0:4788e1df7b55 | 27 | |
rwcjoliver | 0:4788e1df7b55 | 28 | // FUNCTIONS |
rwcjoliver | 0:4788e1df7b55 | 29 | |
jamesmcildowietfl | 3:32e951e05a5b | 30 | //Display Function for data logger |
jamesmcildowietfl | 18:d28d458824d4 | 31 | void DisplaySerial(){ |
jamesmcildowietfl | 18:d28d458824d4 | 32 | std::stringstream displayline; |
jamesmcildowietfl | 19:a4afd3a6bdfb | 33 | displayline << "Blackbox# " << " Motor Accelerator: " << motorAccelerator << " Brake 3/2: " << brakeValve32 << " Brake 2/2: " << brakeValve22 << " Speed: " << dashboard.currentSpeed << " Distance: " << dashboard.currentDistance << " Drive Mode: " << driveMode << "\n"; |
jamesmcildowietfl | 20:0add93a69df5 | 34 | string disp = displayline.str(); |
jamesmcildowietfl | 20:0add93a69df5 | 35 | pc.printf("%s \n", disp.c_str()); |
jamesmcildowietfl | 18:d28d458824d4 | 36 | } |
jamesmcildowietfl | 18:d28d458824d4 | 37 | |
jamesmcildowietfl | 18:d28d458824d4 | 38 | //Emergency Stop |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 39 | void emergencyStop() { //Emergency Stop Function |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 40 | // pc.printf("Emergency Stop Active\r\n"); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 41 | if (emergencyStopActive == false) { |
jamesmcildowietfl | 3:32e951e05a5b | 42 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 43 | emergencyStopActive = true; |
jamesmcildowietfl | 18:d28d458824d4 | 44 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 45 | //Set brake throttle to zero before disconnected, think is why we had the runaway train imo |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 46 | motor1.throttle(0); |
rwcjoliver | 0:4788e1df7b55 | 47 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 48 | //Disengage motor |
jamesmcildowietfl | 18:d28d458824d4 | 49 | motor1.disengage(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 50 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 51 | //Setting brakes to high |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 52 | brakeValve32 = 0;//(PF_2) |
jamesmcildowietfl | 18:d28d458824d4 | 53 | brakeValve22 = 0;//(PF_8) |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 54 | if (rtc_output.read() == 1) { //Check RTC pin out |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 55 | rtc.getTriggerCause(); // Get RTC input status |
rwcjoliver | 0:4788e1df7b55 | 56 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 57 | } |
rwcjoliver | 0:4788e1df7b55 | 58 | } |
rwcjoliver | 0:4788e1df7b55 | 59 | |
jamesmcildowietfl | 18:d28d458824d4 | 60 | //Brake code |
cdevarakonda | 27:5f801d4ca461 | 61 | void brakeControl(int brakeRate) { //set brake rate to float for regen |
jamesmcildowietfl | 18:d28d458824d4 | 62 | if (driveMode == 2) { // PARK MODE |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 63 | // All Mechanical brakes applied |
jamesmcildowietfl | 18:d28d458824d4 | 64 | motor1.throttle(0.0f); |
jamesmcildowietfl | 18:d28d458824d4 | 65 | brakeValve32 = 0; |
jamesmcildowietfl | 18:d28d458824d4 | 66 | brakeValve22 = 0; |
cdevarakonda | 29:2ed4d9c309fc | 67 | //inParkMode=true; |
cdevarakonda | 28:1086791972d0 | 68 | //brakes1.BrakesOn(); |
jamesmcildowietfl | 18:d28d458824d4 | 69 | } |
jamesmcildowietfl | 18:d28d458824d4 | 70 | else {//REGEN BRAKING |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 71 | if (challenge.regenBrakingActive == true) { // REGEN BRAKING WITH OVERVOLTAGE SAFETY CHECK |
cdevarakonda | 25:1e42ee248b89 | 72 | //motor1.setPark(); |
cdevarakonda | 27:5f801d4ca461 | 73 | switch (brakeRate) { //using switch cases like throttle to set regen braking brake tate with values between 0.1f and 1.0f which represents analog pin out values upto 3.3V |
cdevarakonda | 27:5f801d4ca461 | 74 | |
cdevarakonda | 27:5f801d4ca461 | 75 | |
cdevarakonda | 27:5f801d4ca461 | 76 | default: |
cdevarakonda | 27:5f801d4ca461 | 77 | break; |
cdevarakonda | 27:5f801d4ca461 | 78 | |
cdevarakonda | 27:5f801d4ca461 | 79 | case 0: |
cdevarakonda | 27:5f801d4ca461 | 80 | motor1.brake(0.0f); |
cdevarakonda | 27:5f801d4ca461 | 81 | break; |
cdevarakonda | 27:5f801d4ca461 | 82 | |
cdevarakonda | 27:5f801d4ca461 | 83 | case 1: |
cdevarakonda | 27:5f801d4ca461 | 84 | motor1.brake(0.25f); |
cdevarakonda | 27:5f801d4ca461 | 85 | break; |
cdevarakonda | 27:5f801d4ca461 | 86 | |
cdevarakonda | 27:5f801d4ca461 | 87 | case 2: |
cdevarakonda | 27:5f801d4ca461 | 88 | motor1.brake(0.5f); |
cdevarakonda | 27:5f801d4ca461 | 89 | break; |
cdevarakonda | 27:5f801d4ca461 | 90 | |
cdevarakonda | 27:5f801d4ca461 | 91 | case 3: |
cdevarakonda | 27:5f801d4ca461 | 92 | motor1.brake(0.75f); |
cdevarakonda | 27:5f801d4ca461 | 93 | break; |
cdevarakonda | 27:5f801d4ca461 | 94 | |
cdevarakonda | 27:5f801d4ca461 | 95 | case 4: |
cdevarakonda | 27:5f801d4ca461 | 96 | motor1.brake(1.0f); |
cdevarakonda | 27:5f801d4ca461 | 97 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 98 | } |
cdevarakonda | 26:6275acef6bfd | 99 | //else { |
cdevarakonda | 26:6275acef6bfd | 100 | //motor1.setForward(); |
cdevarakonda | 28:1086791972d0 | 101 | } |
cdevarakonda | 27:5f801d4ca461 | 102 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 103 | else { // MECHANICAL BRAKING |
cdevarakonda | 27:5f801d4ca461 | 104 | // int bR=static_cast <int>(brakeRate); //type casting brake rate to int for mechanical cases |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 105 | switch (brakeRate) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 106 | case 0: // NO BRAKING |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 107 | brakeValve32 = 1;//(PF_2) |
jamesmcildowietfl | 18:d28d458824d4 | 108 | brakeValve22 = 1;//(PF_8) |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 109 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 110 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 111 | case 1: //HALF BRAKING |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 112 | motor1.throttle(0.0f); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 113 | brakeValve32 = 0;//(PF_2) |
jamesmcildowietfl | 18:d28d458824d4 | 114 | brakeValve22 = 1;//(PF_8) |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 115 | break; |
jamesmcildowietfl | 2:d8d92dfc9c82 | 116 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 117 | case 2 ... 4 : //FULL BRAKING |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 118 | motor1.throttle(0.0f); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 119 | brakeValve32 = 0;//(PF_2) |
jamesmcildowietfl | 18:d28d458824d4 | 120 | brakeValve22 = 0;//(PF_8) |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 121 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 122 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 123 | default: // NO BRAKING |
jamesmcildowietfl | 18:d28d458824d4 | 124 | brakeValve32 = 1;//(PF_2) |
jamesmcildowietfl | 18:d28d458824d4 | 125 | brakeValve22 = 1;//(PF_8) |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 126 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 127 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 128 | } |
jamesmcildowietfl | 18:d28d458824d4 | 129 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 130 | return; |
rwcjoliver | 0:4788e1df7b55 | 131 | } |
rwcjoliver | 0:4788e1df7b55 | 132 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 133 | //Motor code |
rwcjoliver | 0:4788e1df7b55 | 134 | void speedControl(int commandedSpeed) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 135 | switch (commandedSpeed) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 136 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 137 | default: |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 138 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 139 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 140 | case 0: |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 141 | motor1.throttle(0.0f); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 142 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 143 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 144 | case 1 ... 2: |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 145 | motor1.throttle(0.1f); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 146 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 147 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 148 | case 3 ... 4: |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 149 | motor1.throttle(0.2f); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 150 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 151 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 152 | case 5 ... 6: |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 153 | motor1.throttle(0.3f); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 154 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 155 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 156 | case 7 ... 8: |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 157 | motor1.throttle(0.4f); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 158 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 159 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 160 | case 9 ... 10: |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 161 | motor1.throttle(0.5f); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 162 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 163 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 164 | case 11: |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 165 | motor1.throttle(0.6f); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 166 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 167 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 168 | case 12: |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 169 | motor1.throttle(0.7f); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 170 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 171 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 172 | case 13: |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 173 | motor1.throttle(0.8f); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 174 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 175 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 176 | case 14: |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 177 | motor1.throttle(0.9f); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 178 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 179 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 180 | case 15: |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 181 | motor1.throttle(1.0f); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 182 | break; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 183 | } |
rwcjoliver | 0:4788e1df7b55 | 184 | } |
rwcjoliver | 0:4788e1df7b55 | 185 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 186 | int main() { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 187 | pc.baud(115200); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 188 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 189 | // CONFIGURE INTERRUPTS |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 190 | rtc_output.rise(&emergencyStop); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 191 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 192 | millisStart(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 193 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 194 | rtc_Trigger = 1; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 195 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 196 | // LOCAL VARIABLES |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 197 | bool systemOn = false; // On/Off status of loco |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 198 | int lastKnownDirection = 2; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 199 | bool inParkMode = false; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 200 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 201 | // Record last time error was sent |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 202 | unsigned long lastErrorMillis = 0; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 203 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 204 | //MainLoop |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 205 | while (1) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 206 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 207 | while (remote.commsGood == true) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 208 | /////Start Up/////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 209 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 210 | /////Checking Modes from controller///////////////////////////////////////////////////////////////////////////////// |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 211 | // PING |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 212 | remote.commsCheck(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 213 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 214 | // GET SWITCH STATES |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 215 | remote.getSwitchStates(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 216 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 217 | // ALLOW BRAKES TO BE OPERATED |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 218 | brakeControl(remote.braking); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 219 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 220 | // SOUND WHISTLE IF WHISTLE BUTTON PRESSED |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 221 | if (remote.whistle == 0) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 222 | whistleValve32 = 1; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 223 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 224 | else { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 225 | whistleValve32 = 0; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 226 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 227 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 228 | // GET AND DISPLAY SPEED |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 229 | dashboard.getCurrentSpeed(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 230 | remote.sendData(2, dashboard.currentSpeed); // Send speed to remote |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 231 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 232 | // TOGGLE COMPRESSOR |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 233 | remote.compressor == 0 ? contactCompressor = 1 : contactCompressor = 0; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 234 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 235 | // TOGGLE MOTOR CONTROLLER DEADMAN (SEAT SWITCH AND FOOTBRAKE) |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 236 | if (rtc.deadman == 0) { // IF DEADMAN PRESSED |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 237 | motor1.closeDeadman(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 238 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 239 | else { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 240 | motor1.releaseDeadman(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 241 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 242 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 243 | // TOGGLE REGEN THROTTLING |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 244 | if (challenge.regenThrottleActive == false) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 245 | if (remote.regenThrottle == 0 && remote.start == 0) { // TURN OFF IF ON |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 246 | challenge.regenThrottleOn(); |
rwcjoliver | 0:4788e1df7b55 | 247 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 248 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 249 | else { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 250 | remote.sendError('B'); // Send error to remote |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 251 | if (remote.regenThrottle == 1) { // TURN ON IF OFF |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 252 | challenge.regenThrottleOff(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 253 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 254 | } |
rwcjoliver | 0:4788e1df7b55 | 255 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 256 | // TOGGLE REGEN BRAKING |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 257 | if (challenge.regenBrakingActive == false) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 258 | if (remote.regenBrake == 0 && remote.start == 0) { // TURN OFF IF ON |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 259 | if (challenge.regenBrakingOn() == 0) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 260 | remote.sendError('I'); // Send error to remote |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 261 | pc.printf("Regen Braking Off - SuperCaps are full\r\n"); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 262 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 263 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 264 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 265 | else { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 266 | remote.sendError('C'); // Send error to remote |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 267 | if (remote.regenBrake == 1) { // TURN OFF |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 268 | challenge.regenBrakingOff(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 269 | if (superCapVoltage == 1) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 270 | pc.printf("Regen Braking Off - SuperCaps are full\r\n"); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 271 | remote.sendError('I'); // Send error to remote |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 272 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 273 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 274 | } |
rwcjoliver | 0:4788e1df7b55 | 275 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 276 | // TOGGLE AUTOSTOP |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 277 | if (challenge.autoStopActive == 0) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 278 | if (remote.autoStop == 0 && remote.start == 0) { // TURN OFF IF ON |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 279 | challenge.autoStopOn(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 280 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 281 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 282 | else { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 283 | remote.sendError('D'); // Send error to remote |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 284 | if (remote.autoStop == 1) { // TURN ON IF OFF |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 285 | challenge.autoStopOff(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 286 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 287 | } |
rwcjoliver | 0:4788e1df7b55 | 288 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 289 | // TOGGLE INNOVATION |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 290 | if (challenge.innovationActive == 0) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 291 | if (remote.innovation == 0 && remote.start == 0) { // TURN OFF IF ON |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 292 | if (driveMode == 0) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 293 | challenge.innovationOn(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 294 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 295 | else { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 296 | remote.sendError('J'); // Send error to remote |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 297 | pc.printf("Can only active innovation mode in forward direction\r\n"); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 298 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 299 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 300 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 301 | else { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 302 | remote.sendError('E'); // Send error to remote |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 303 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 304 | if (remote.innovation == 1) { // TURN ON IF OFF |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 305 | challenge.innovationOff(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 306 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 307 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 308 | /////END OF TOGGLE CHECKS////////////////////////////////////////////////////////////////////////////////////////////// |
rwcjoliver | 0:4788e1df7b55 | 309 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 310 | /////Control/////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 311 | //Is a Toggle check, but it is the train start swtich A |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 312 | if (systemOn == false) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 313 | if (remote.start == 1) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 314 | if (millis() - lastErrorMillis > 500) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 315 | remote.sendError('A'); // SEND ERROR MESSAGE 'A' TO REMOTE |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 316 | lastErrorMillis = millis(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 317 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 318 | motor1.turnOff(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 319 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 320 | else { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 321 | systemOn = true; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 322 | motor1.turnOn(); // Turn on motors |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 323 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 324 | } // END IF SYSTEMON = FALSE |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 325 | //If train is switched on and in start do this start/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 326 | else { // IF SYSTEMON == TRUE |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 327 | if (remote.start == 1) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 328 | systemOn = false; // WILL STOP ABOVE HERE NEXT LOOP |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 329 | pc.printf("Start Switch is Off\r\n"); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 330 | } |
jamesmcildowietfl | 15:4976d145fbd9 | 331 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 332 | //Set foward |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 333 | if (driveMode != 0 && remote.forward == 0) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 334 | driveMode = 0; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 335 | motor1.setForward(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 336 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 337 | //Set reverse |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 338 | if (driveMode != 1 && remote.reverse == 0) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 339 | driveMode = 1; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 340 | motor1.setReverse(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 341 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 342 | //Set park |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 343 | if (driveMode != 2 && remote.park == 0) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 344 | driveMode = 2; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 345 | motor1.setPark(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 346 | motor1.throttle(0); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 347 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 348 | ////Park Mode |
cdevarakonda | 29:2ed4d9c309fc | 349 | if (driveMode == 2) { |
cdevarakonda | 29:2ed4d9c309fc | 350 | brakeControl(4); //place in park mode if selected by driver |
jamesmcildowietfl | 18:d28d458824d4 | 351 | if (inParkMode == false) { |
jamesmcildowietfl | 18:d28d458824d4 | 352 | pc.printf("Train in park mode.\r\n"); //why? |
jamesmcildowietfl | 18:d28d458824d4 | 353 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 354 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 355 | if (emergencyStopActive == true && rtc_output.read() == 0) { // Clear emergency stop flag |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 356 | emergencyStopActive = false; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 357 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 358 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 359 | led_parkMode = 1; |
jamesmcildowietfl | 18:d28d458824d4 | 360 | inParkMode = true; // Stop above debug print from displaying more than once |
jamesmcildowietfl | 18:d28d458824d4 | 361 | motor1.setPark(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 362 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 363 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 364 | ////Drive |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 365 | else { //else i.e if selected drive mode is forward or reverse |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 366 | ////////////////// Start of check for error G RTC clear |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 367 | if (emergencyStopActive == false && rtc_output.read() == 0) { // IF RTC FLAGGED AS SAFE |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 368 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 369 | led_parkMode = 0; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 370 | inParkMode = false; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 371 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 372 | if (driveMode != lastKnownDirection) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 373 | pc.printf("Train enabled for direction %d\r\n", driveMode); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 374 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 375 | lastKnownDirection = driveMode; |
rwcjoliver | 0:4788e1df7b55 | 376 | } |
rwcjoliver | 0:4788e1df7b55 | 377 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 378 | ////Call autostop challenge |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 379 | if (challenge.autoStopInProgress == true) { // IF AUTOSTOPPING, PASS THROTTLE CONTROL TO FUNCTION |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 380 | challenge.autoStopControl(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 381 | pc.printf("Autostop in Control\r\n"); |
rwcjoliver | 0:4788e1df7b55 | 382 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 383 | //Use controls from remote |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 384 | else { // OTHERWISE INPUT THROTTLE FROM REMOTE |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 385 | if (remote.throttle > 0) { // If joystick pushed upwards = throttle |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 386 | /////////////////////////Innovation braking start |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 387 | if (challenge.innovationActive == true) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 388 | pc.printf("Collision Detection in Control\r\n"); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 389 | int innovationThrottle = challenge.innovationControl(remote.throttle); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 390 | speedControl(innovationThrottle); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 391 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 392 | if (innovationThrottle == 0) { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 393 | emergencyStop(); // emergency Brake if obstacle detected |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 394 | } |
rwcjoliver | 0:4788e1df7b55 | 395 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 396 | /////////////////////////Innovation braking end |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 397 | //normal throttle control |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 398 | else { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 399 | speedControl(remote.throttle); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 400 | pc.printf("Throttling: %d\r\n", remote.throttle); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 401 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 402 | } // remote.throttle |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 403 | ///if nothing set to 0 |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 404 | else { |
jamesmcildowietfl | 18:d28d458824d4 | 405 | speedControl(0); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 406 | } |
rwcjoliver | 0:4788e1df7b55 | 407 | } |
rwcjoliver | 0:4788e1df7b55 | 408 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 409 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 410 | ////////////////// End of check for error G RTC clear |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 411 | else { |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 412 | pc.printf("Cannot exit park mode until RTC is cleared\r\n"); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 413 | inParkMode = false; |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 414 | remote.sendError('G'); // Send error to remote |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 415 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 416 | } |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 417 | //Datalogger Chai Funciton |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 418 | DisplaySerial(); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 419 | } // END IF (SYSTEMON == TRUE) |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 420 | //If train is switched on and in start do this end/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
rwcjoliver | 0:4788e1df7b55 | 421 | |
jamesmcildowietfl | 19:a4afd3a6bdfb | 422 | wait_ms(25); // SLOW DOWN THE SYSTEM (REMOTE CANT KEEP UP) |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 423 | } // END WHILE(COMMSGOOD) |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 424 | pc.printf("Main Loop Skipped Due To Emergency Status\r\n"); |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 425 | emergencyStop(); // Emergency stop if comms lost with remote controller |
rwcjoliver | 0:4788e1df7b55 | 426 | |
rwcjoliver | 0:4788e1df7b55 | 427 | |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 428 | } //END WHILE(1) |
jamesmcildowietfl | 8:0fe9f7bde2f9 | 429 | } |