Railway Challenge / Mbed 2 deprecated challenge-ChaiUpdated-AutostopTest-30Throttle

This is a test from 30%Throttle to stop, using full service brake

Dependencies:   mbed millis

main.cpp@0:4788e1df7b55, 2020-03-13 (annotated)

Committer:
rwcjoliver
Date:
Fri Mar 13 11:48:04 2020 +0000
Revision:
0:4788e1df7b55
Child:
1:ba85dae98035
Uncommented RegenBRaking lines to allow capacitors to charge up from the batteries

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rwcjoliver 0:4788e1df7b55 1 #include <mbed.h>
rwcjoliver 0:4788e1df7b55 2 #include "millis.h"
rwcjoliver 0:4788e1df7b55 3
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"
rwcjoliver 0:4788e1df7b55 10
rwcjoliver 0:4788e1df7b55 11 // SET UP REMOTE CONTROL COMMS
rwcjoliver 0:4788e1df7b55 12 SPI remoteControl(PE_14, PE_13, PE_12); // (SPI_MOSI, SPI_MISO, SPI_SCK)
rwcjoliver 0:4788e1df7b55 13 DigitalOut remoteControlCS(PE_11); // (SPI_SS)
rwcjoliver 0:4788e1df7b55 14
rwcjoliver 0:4788e1df7b55 15 // CREATE OBJECTS
rwcjoliver 0:4788e1df7b55 16 Remote remote(remoteControl, remoteControlCS);
rwcjoliver 0:4788e1df7b55 17 Dashboard dashboard(hallSensor);
rwcjoliver 0:4788e1df7b55 18 RoundTrainCircuit rtc(rtc_1, rtc_2, rtc_3, rtc_4, rtc_5, rtc_6, rtc_7, rtc_override);
rwcjoliver 0:4788e1df7b55 19 Motor motor1(motorAccelerator, motorBrake, keySwitchM1, directionFwd, directionRev, footswitchM1, seatM1, inchFwdM1, speedLimit2M1, speedLimit3M1);
rwcjoliver 0:4788e1df7b55 20 ChallengeMode challenge(autoStopTrigger, dashboard, remote, motor1);
rwcjoliver 0:4788e1df7b55 21
rwcjoliver 0:4788e1df7b55 22 int driveMode = 2; // Drive mode - fwd(0), rev(1), park(2)
rwcjoliver 0:4788e1df7b55 23 bool emergencyStopActive = false;
rwcjoliver 0:4788e1df7b55 24
rwcjoliver 0:4788e1df7b55 25 // FUNCTIONS
rwcjoliver 0:4788e1df7b55 26
rwcjoliver 0:4788e1df7b55 27 void startupHealthCheck() {
rwcjoliver 0:4788e1df7b55 28
rwcjoliver 0:4788e1df7b55 29 while(1) {
rwcjoliver 0:4788e1df7b55 30
rwcjoliver 0:4788e1df7b55 31 if (remote.commsGood == 1) {
rwcjoliver 0:4788e1df7b55 32 if (rtc_output.read() == 0) {
rwcjoliver 0:4788e1df7b55 33 // if (batteryVoltage == true) {
rwcjoliver 0:4788e1df7b55 34 if (superCapVoltage == true) {
rwcjoliver 0:4788e1df7b55 35 //
rwcjoliver 0:4788e1df7b55 36 //
rwcjoliver 0:4788e1df7b55 37 return; // Exit the function if all checks are passed
rwcjoliver 0:4788e1df7b55 38 //
rwcjoliver 0:4788e1df7b55 39 }
rwcjoliver 0:4788e1df7b55 40 else {
rwcjoliver 0:4788e1df7b55 41 pc.printf("System Start-Up Health Check: SuperCap Voltage Check Failed\r\n");
rwcjoliver 0:4788e1df7b55 42 }
rwcjoliver 0:4788e1df7b55 43 // }
rwcjoliver 0:4788e1df7b55 44 // else {
rwcjoliver 0:4788e1df7b55 45 // pc.printf("System Start-Up Health Check: Battery Voltage Check Failed\r\n");
rwcjoliver 0:4788e1df7b55 46 // }
rwcjoliver 0:4788e1df7b55 47 }
rwcjoliver 0:4788e1df7b55 48 else {
rwcjoliver 0:4788e1df7b55 49 pc.printf("System Start-Up Health Check: RTC Check Failed\r\n");
rwcjoliver 0:4788e1df7b55 50 }
rwcjoliver 0:4788e1df7b55 51 }
rwcjoliver 0:4788e1df7b55 52 else {
rwcjoliver 0:4788e1df7b55 53 pc.printf("System Start-Up Health Check: CommsCheck Failed\r\n");
rwcjoliver 0:4788e1df7b55 54 }
rwcjoliver 0:4788e1df7b55 55 remote.sendError('H'); // Send error to remote
rwcjoliver 0:4788e1df7b55 56 wait(100); // Wait a while until trying again
rwcjoliver 0:4788e1df7b55 57 }
rwcjoliver 0:4788e1df7b55 58 }
rwcjoliver 0:4788e1df7b55 59
rwcjoliver 0:4788e1df7b55 60 void emergencyStop() {
rwcjoliver 0:4788e1df7b55 61 // pc.printf("Emergency Stop Active\r\n");
rwcjoliver 0:4788e1df7b55 62 if (emergencyStopActive == false) {
rwcjoliver 0:4788e1df7b55 63
rwcjoliver 0:4788e1df7b55 64 emergencyStopActive = true;
rwcjoliver 0:4788e1df7b55 65
rwcjoliver 0:4788e1df7b55 66 motor1.disengage(); // Disengage both motors
rwcjoliver 0:4788e1df7b55 67 // motor2.disengage();
rwcjoliver 0:4788e1df7b55 68
rwcjoliver 0:4788e1df7b55 69 motor1.setPark(); // Clear Motor Directions
rwcjoliver 0:4788e1df7b55 70 // motor2.setPark();
rwcjoliver 0:4788e1df7b55 71
rwcjoliver 0:4788e1df7b55 72 if (rtc_output.read() == 1) {
rwcjoliver 0:4788e1df7b55 73 rtc.getTriggerCause(); // Get RTC input status
rwcjoliver 0:4788e1df7b55 74 }
rwcjoliver 0:4788e1df7b55 75 }
rwcjoliver 0:4788e1df7b55 76 }
rwcjoliver 0:4788e1df7b55 77
rwcjoliver 0:4788e1df7b55 78 // Prototype"!!!!
rwcjoliver 0:4788e1df7b55 79 void speedControl(int);
rwcjoliver 0:4788e1df7b55 80
rwcjoliver 0:4788e1df7b55 81 void brakeControl(int brakeRate) {
rwcjoliver 0:4788e1df7b55 82 if (driveMode == 2) { // PARK MODE
rwcjoliver 0:4788e1df7b55 83 // BREAK RATE LEVEL 1
rwcjoliver 0:4788e1df7b55 84 speedControl(0);
rwcjoliver 0:4788e1df7b55 85 brakeValve32 = 0;
rwcjoliver 0:4788e1df7b55 86 brakeValve22 = 1;
rwcjoliver 0:4788e1df7b55 87 //if (pressureSwitch1 == 0) {
rwcjoliver 0:4788e1df7b55 88 // brakeValve22 = 0;
rwcjoliver 0:4788e1df7b55 89 // }
rwcjoliver 0:4788e1df7b55 90 // else {
rwcjoliver 0:4788e1df7b55 91 // brakeValve22 = 1;
rwcjoliver 0:4788e1df7b55 92 // }
rwcjoliver 0:4788e1df7b55 93 }
rwcjoliver 0:4788e1df7b55 94 else {
rwcjoliver 0:4788e1df7b55 95
rwcjoliver 0:4788e1df7b55 96 if (challenge.regenBrakingActive == true) { // REGEN BRAKING WITH OVERVOLTAGE SAFETY CHECK
rwcjoliver 0:4788e1df7b55 97 if (brakeRate > 0) {
rwcjoliver 0:4788e1df7b55 98 motor1.setPark();
rwcjoliver 0:4788e1df7b55 99 }
rwcjoliver 0:4788e1df7b55 100 else {
rwcjoliver 0:4788e1df7b55 101 motor1.setForward();
rwcjoliver 0:4788e1df7b55 102 }
rwcjoliver 0:4788e1df7b55 103 // switch (brakeRate) {
rwcjoliver 0:4788e1df7b55 104 // case 0: // NO BRAKING
rwcjoliver 0:4788e1df7b55 105 // motor1.brake(0.00f);
rwcjoliver 0:4788e1df7b55 106 //// motor2.brake(0.00f);
rwcjoliver 0:4788e1df7b55 107 //// contactBatt = 0; // Close battery contactor so all power comes from supercaps
rwcjoliver 0:4788e1df7b55 108 // pc.printf("Regen Braking set to 0%\r\n");
rwcjoliver 0:4788e1df7b55 109 // break;
rwcjoliver 0:4788e1df7b55 110 //
rwcjoliver 0:4788e1df7b55 111 // case 1:
rwcjoliver 0:4788e1df7b55 112 // motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 113 // motor1.brake(0.33f);
rwcjoliver 0:4788e1df7b55 114 //// motor2.brake(0.33f);
rwcjoliver 0:4788e1df7b55 115 //// contactBatt = 0; // Open battery contactor so all power comes from supercaps
rwcjoliver 0:4788e1df7b55 116 // pc.printf("Regen Braking set to 33%\r\n");
rwcjoliver 0:4788e1df7b55 117 // break;
rwcjoliver 0:4788e1df7b55 118 //
rwcjoliver 0:4788e1df7b55 119 // case 2:
rwcjoliver 0:4788e1df7b55 120 // motor1.brake(0.66f);
rwcjoliver 0:4788e1df7b55 121 // motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 122 //// motor2.brake(0.66f);
rwcjoliver 0:4788e1df7b55 123 //// contactBatt = 0; // Open battery contactor so all power comes from supercaps
rwcjoliver 0:4788e1df7b55 124 // pc.printf("Regen Braking set to 66%\r\n");
rwcjoliver 0:4788e1df7b55 125 // break;
rwcjoliver 0:4788e1df7b55 126 //
rwcjoliver 0:4788e1df7b55 127 // case 3:
rwcjoliver 0:4788e1df7b55 128 // motor1.brake(1.0f);
rwcjoliver 0:4788e1df7b55 129 // motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 130 //// motor2.brake(1.0f);
rwcjoliver 0:4788e1df7b55 131 // pc.printf("Regen Braking set to 100%\r\n");
rwcjoliver 0:4788e1df7b55 132 // break;
rwcjoliver 0:4788e1df7b55 133 //
rwcjoliver 0:4788e1df7b55 134 // default:
rwcjoliver 0:4788e1df7b55 135 // break;
rwcjoliver 0:4788e1df7b55 136 // }
rwcjoliver 0:4788e1df7b55 137 }
rwcjoliver 0:4788e1df7b55 138 else { // MECHANICAL BRAKING
rwcjoliver 0:4788e1df7b55 139 switch (brakeRate) {
rwcjoliver 0:4788e1df7b55 140 case 0: // NO BRAKING
rwcjoliver 0:4788e1df7b55 141 brakeValve32 = 1;
rwcjoliver 0:4788e1df7b55 142 brakeValve22 = 1;
rwcjoliver 0:4788e1df7b55 143 break;
rwcjoliver 0:4788e1df7b55 144
rwcjoliver 0:4788e1df7b55 145 case 1:
rwcjoliver 0:4788e1df7b55 146 motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 147 brakeValve32 = 0;
rwcjoliver 0:4788e1df7b55 148 if (pressureSwitch1.read() == 0) {
rwcjoliver 0:4788e1df7b55 149 brakeValve22 = 0;
rwcjoliver 0:4788e1df7b55 150 pc.printf("Pressure 1 Reached");
rwcjoliver 0:4788e1df7b55 151 }
rwcjoliver 0:4788e1df7b55 152 else {
rwcjoliver 0:4788e1df7b55 153 brakeValve22 = 1;
rwcjoliver 0:4788e1df7b55 154 pc.printf("Braking Level 1\r\n");
rwcjoliver 0:4788e1df7b55 155 }
rwcjoliver 0:4788e1df7b55 156 break;
rwcjoliver 0:4788e1df7b55 157
rwcjoliver 0:4788e1df7b55 158 // case 2:
rwcjoliver 0:4788e1df7b55 159 // motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 160 // brakeValve32 = 0;
rwcjoliver 0:4788e1df7b55 161 // if (pressureSwitch2.read() == 0) {
rwcjoliver 0:4788e1df7b55 162 // brakeValve22 = 0;
rwcjoliver 0:4788e1df7b55 163 // pc.printf("Pressure 2 Reached");
rwcjoliver 0:4788e1df7b55 164 // }
rwcjoliver 0:4788e1df7b55 165 // else {
rwcjoliver 0:4788e1df7b55 166 // brakeValve22 = 1;
rwcjoliver 0:4788e1df7b55 167 // pc.printf("Braking Level 2\r\n");
rwcjoliver 0:4788e1df7b55 168 // }
rwcjoliver 0:4788e1df7b55 169 //
rwcjoliver 0:4788e1df7b55 170 // break;
rwcjoliver 0:4788e1df7b55 171
rwcjoliver 0:4788e1df7b55 172 case 2 ... 4:
rwcjoliver 0:4788e1df7b55 173 motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 174 brakeValve32 = 0;
rwcjoliver 0:4788e1df7b55 175 brakeValve22 = 1;
rwcjoliver 0:4788e1df7b55 176
rwcjoliver 0:4788e1df7b55 177 // if (pressureSwitch3.read() == 0) {
rwcjoliver 0:4788e1df7b55 178 // brakeValve22 = 0;
rwcjoliver 0:4788e1df7b55 179 // pc.printf("Pressure 3 Reached");
rwcjoliver 0:4788e1df7b55 180 // }
rwcjoliver 0:4788e1df7b55 181 // else {
rwcjoliver 0:4788e1df7b55 182 // brakeValve22 = 1;
rwcjoliver 0:4788e1df7b55 183 // pc.printf("Braking Level 3\r\n");
rwcjoliver 0:4788e1df7b55 184 // }
rwcjoliver 0:4788e1df7b55 185 break;
rwcjoliver 0:4788e1df7b55 186
rwcjoliver 0:4788e1df7b55 187
rwcjoliver 0:4788e1df7b55 188 default: // NO BRAKING
rwcjoliver 0:4788e1df7b55 189 brakeValve32 = 1;
rwcjoliver 0:4788e1df7b55 190 brakeValve22 = 1;
rwcjoliver 0:4788e1df7b55 191 break;
rwcjoliver 0:4788e1df7b55 192 }
rwcjoliver 0:4788e1df7b55 193 }
rwcjoliver 0:4788e1df7b55 194 }
rwcjoliver 0:4788e1df7b55 195 return;
rwcjoliver 0:4788e1df7b55 196 }
rwcjoliver 0:4788e1df7b55 197
rwcjoliver 0:4788e1df7b55 198 void speedControl(int commandedSpeed) {
rwcjoliver 0:4788e1df7b55 199 if (dashboard.currentSpeed < 16.00) { // IF SPEED LESS THAN LIMIT
rwcjoliver 0:4788e1df7b55 200 switch (commandedSpeed) {
rwcjoliver 0:4788e1df7b55 201
rwcjoliver 0:4788e1df7b55 202 default:
rwcjoliver 0:4788e1df7b55 203 // motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 204 break;
rwcjoliver 0:4788e1df7b55 205
rwcjoliver 0:4788e1df7b55 206 case 0:
rwcjoliver 0:4788e1df7b55 207 motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 208 break;
rwcjoliver 0:4788e1df7b55 209
rwcjoliver 0:4788e1df7b55 210 case 1 ... 2:
rwcjoliver 0:4788e1df7b55 211 motor1.throttle(0.1f);
rwcjoliver 0:4788e1df7b55 212 break;
rwcjoliver 0:4788e1df7b55 213
rwcjoliver 0:4788e1df7b55 214 case 3 ... 4:
rwcjoliver 0:4788e1df7b55 215 motor1.throttle(0.2f);
rwcjoliver 0:4788e1df7b55 216 break;
rwcjoliver 0:4788e1df7b55 217
rwcjoliver 0:4788e1df7b55 218 case 5 ... 6:
rwcjoliver 0:4788e1df7b55 219 motor1.throttle(0.3f);
rwcjoliver 0:4788e1df7b55 220 break;
rwcjoliver 0:4788e1df7b55 221
rwcjoliver 0:4788e1df7b55 222 case 7 ... 8:
rwcjoliver 0:4788e1df7b55 223 motor1.throttle(0.4f);
rwcjoliver 0:4788e1df7b55 224 break;
rwcjoliver 0:4788e1df7b55 225
rwcjoliver 0:4788e1df7b55 226 case 9 ... 10:
rwcjoliver 0:4788e1df7b55 227 motor1.throttle(0.5f);
rwcjoliver 0:4788e1df7b55 228 break;
rwcjoliver 0:4788e1df7b55 229
rwcjoliver 0:4788e1df7b55 230 case 11:
rwcjoliver 0:4788e1df7b55 231 motor1.throttle(0.6f);
rwcjoliver 0:4788e1df7b55 232 break;
rwcjoliver 0:4788e1df7b55 233
rwcjoliver 0:4788e1df7b55 234 case 12:
rwcjoliver 0:4788e1df7b55 235 motor1.throttle(0.7f);
rwcjoliver 0:4788e1df7b55 236 break;
rwcjoliver 0:4788e1df7b55 237
rwcjoliver 0:4788e1df7b55 238 case 13:
rwcjoliver 0:4788e1df7b55 239 motor1.throttle(0.8f);
rwcjoliver 0:4788e1df7b55 240 break;
rwcjoliver 0:4788e1df7b55 241
rwcjoliver 0:4788e1df7b55 242 case 14:
rwcjoliver 0:4788e1df7b55 243 motor1.throttle(0.9f);
rwcjoliver 0:4788e1df7b55 244 break;
rwcjoliver 0:4788e1df7b55 245
rwcjoliver 0:4788e1df7b55 246 case 15:
rwcjoliver 0:4788e1df7b55 247 motor1.throttle(1.0f);
rwcjoliver 0:4788e1df7b55 248 break;
rwcjoliver 0:4788e1df7b55 249 }
rwcjoliver 0:4788e1df7b55 250 }
rwcjoliver 0:4788e1df7b55 251 else { // IF OVER 15KPH
rwcjoliver 0:4788e1df7b55 252 if (dashboard.currentSpeed < 20.00) { // If speed less than 20 (we cant physically go this fast so any faster is probably compressor noise), cut throttle, otherwise ignore
rwcjoliver 0:4788e1df7b55 253 motor1.throttle(0.0f); // COMMENTED AS ALREADY SET 0 IN BRAKECONTROL
rwcjoliver 0:4788e1df7b55 254 // brakeControl(1);
rwcjoliver 0:4788e1df7b55 255 }
rwcjoliver 0:4788e1df7b55 256 }
rwcjoliver 0:4788e1df7b55 257 }
rwcjoliver 0:4788e1df7b55 258
rwcjoliver 0:4788e1df7b55 259 /*void speedControl(int commandedSpeed) {
rwcjoliver 0:4788e1df7b55 260 if (commandedSpeed > dashboard.currentSpeed) { // IF THROTTLE REQUESTED
rwcjoliver 0:4788e1df7b55 261 // Max possible difference is 15
rwcjoliver 0:4788e1df7b55 262 // Motor Analog Voltage between 0 and 5
rwcjoliver 0:4788e1df7b55 263 // 5 / 15 = 0.33333 = 0.4v / kph difference
rwcjoliver 0:4788e1df7b55 264
rwcjoliver 0:4788e1df7b55 265 int difference = commandedSpeed - dashboard.currentSpeed;
rwcjoliver 0:4788e1df7b55 266
rwcjoliver 0:4788e1df7b55 267 switch (difference) {
rwcjoliver 0:4788e1df7b55 268 case 1:
rwcjoliver 0:4788e1df7b55 269 motor1.throttle(0.1f);
rwcjoliver 0:4788e1df7b55 270 // motor2.throttle(0.1f);
rwcjoliver 0:4788e1df7b55 271 pc.printf("Throttle set to 10%\r\n");
rwcjoliver 0:4788e1df7b55 272 break;
rwcjoliver 0:4788e1df7b55 273
rwcjoliver 0:4788e1df7b55 274 case 2 ... 3:
rwcjoliver 0:4788e1df7b55 275 motor1.throttle(0.2f);
rwcjoliver 0:4788e1df7b55 276 // motor2.throttle(0.2f);
rwcjoliver 0:4788e1df7b55 277 pc.printf("Throttle set to 20%\r\n");
rwcjoliver 0:4788e1df7b55 278 break;
rwcjoliver 0:4788e1df7b55 279
rwcjoliver 0:4788e1df7b55 280 case 4 ... 6:
rwcjoliver 0:4788e1df7b55 281 motor1.throttle(0.4f);
rwcjoliver 0:4788e1df7b55 282 // motor2.throttle(0.4f);
rwcjoliver 0:4788e1df7b55 283 pc.printf("Throttle set to 40%\r\n");
rwcjoliver 0:4788e1df7b55 284 break;
rwcjoliver 0:4788e1df7b55 285
rwcjoliver 0:4788e1df7b55 286 case 7 ... 9:
rwcjoliver 0:4788e1df7b55 287 motor1.throttle(0.6f);
rwcjoliver 0:4788e1df7b55 288 // motor2.throttle(0.6f);
rwcjoliver 0:4788e1df7b55 289 pc.printf("Throttle set to 60%\r\n");
rwcjoliver 0:4788e1df7b55 290 break;
rwcjoliver 0:4788e1df7b55 291
rwcjoliver 0:4788e1df7b55 292 case 10 ... 12:
rwcjoliver 0:4788e1df7b55 293 motor1.throttle(0.8f);
rwcjoliver 0:4788e1df7b55 294 // motor2.throttle(0.8f);
rwcjoliver 0:4788e1df7b55 295 pc.printf("Throttle set to 80%\r\n");
rwcjoliver 0:4788e1df7b55 296 break;
rwcjoliver 0:4788e1df7b55 297
rwcjoliver 0:4788e1df7b55 298 case 13 ... 15:
rwcjoliver 0:4788e1df7b55 299 motor1.throttle(1.0f);
rwcjoliver 0:4788e1df7b55 300 // motor2.throttle(1.0f);
rwcjoliver 0:4788e1df7b55 301 pc.printf("Throttle set to 100%\r\n");
rwcjoliver 0:4788e1df7b55 302 break;
rwcjoliver 0:4788e1df7b55 303
rwcjoliver 0:4788e1df7b55 304 default:
rwcjoliver 0:4788e1df7b55 305 motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 306 break;
rwcjoliver 0:4788e1df7b55 307 }
rwcjoliver 0:4788e1df7b55 308 }
rwcjoliver 0:4788e1df7b55 309 else { // COAST
rwcjoliver 0:4788e1df7b55 310 motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 311 // motor2.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 312 }
rwcjoliver 0:4788e1df7b55 313 }*/
rwcjoliver 0:4788e1df7b55 314
rwcjoliver 0:4788e1df7b55 315 int main() {
rwcjoliver 0:4788e1df7b55 316 pc.baud(115200);
rwcjoliver 0:4788e1df7b55 317
rwcjoliver 0:4788e1df7b55 318 // CONFIGURE INTERRUPTS
rwcjoliver 0:4788e1df7b55 319 rtc_output.rise(&emergencyStop);
rwcjoliver 0:4788e1df7b55 320
rwcjoliver 0:4788e1df7b55 321 millisStart();
rwcjoliver 0:4788e1df7b55 322
rwcjoliver 0:4788e1df7b55 323 rtc_Trigger = 1;
rwcjoliver 0:4788e1df7b55 324
rwcjoliver 0:4788e1df7b55 325 // LOCAL VARIABLES
rwcjoliver 0:4788e1df7b55 326 bool systemOn = false; // On/Off status of loco
rwcjoliver 0:4788e1df7b55 327 int lastKnownDirection = 2;
rwcjoliver 0:4788e1df7b55 328 bool inParkMode = false;
rwcjoliver 0:4788e1df7b55 329
rwcjoliver 0:4788e1df7b55 330 // Record last time error was sent
rwcjoliver 0:4788e1df7b55 331 unsigned long lastErrorMillis = 0;
rwcjoliver 0:4788e1df7b55 332
rwcjoliver 0:4788e1df7b55 333 while(1) {
rwcjoliver 0:4788e1df7b55 334
rwcjoliver 0:4788e1df7b55 335 while(remote.commsGood == true) {
rwcjoliver 0:4788e1df7b55 336
rwcjoliver 0:4788e1df7b55 337 // PING
rwcjoliver 0:4788e1df7b55 338 remote.commsCheck();
rwcjoliver 0:4788e1df7b55 339 // GET SWITCH STATES
rwcjoliver 0:4788e1df7b55 340 remote.getSwitchStates();
rwcjoliver 0:4788e1df7b55 341
rwcjoliver 0:4788e1df7b55 342 // ALLOW BRAKES TO BE OPERATED
rwcjoliver 0:4788e1df7b55 343 brakeControl(remote.braking);
rwcjoliver 0:4788e1df7b55 344
rwcjoliver 0:4788e1df7b55 345 // Print Pressure Switch States (Debugging)
rwcjoliver 0:4788e1df7b55 346 // pc.printf("Pressure Switch 1: %d\r\n", pressureSwitch1.read());
rwcjoliver 0:4788e1df7b55 347 // pc.printf("Pressure Switch 2: %d\r\n", pressureSwitch2.read());
rwcjoliver 0:4788e1df7b55 348 // pc.printf("Pressure Switch 3: %d\r\n", pressureSwitch3.read());
rwcjoliver 0:4788e1df7b55 349
rwcjoliver 0:4788e1df7b55 350 // SOUND WHISTLE IF WHISTLE BUTTON PRESSED
rwcjoliver 0:4788e1df7b55 351 if (remote.whistle == 0) {
rwcjoliver 0:4788e1df7b55 352 whistleValve32 = 1;
rwcjoliver 0:4788e1df7b55 353 // wait(0.5);
rwcjoliver 0:4788e1df7b55 354 // whistleValve32 = 1;
rwcjoliver 0:4788e1df7b55 355 }
rwcjoliver 0:4788e1df7b55 356 else {
rwcjoliver 0:4788e1df7b55 357 whistleValve32 = 0;
rwcjoliver 0:4788e1df7b55 358 }
rwcjoliver 0:4788e1df7b55 359
rwcjoliver 0:4788e1df7b55 360
rwcjoliver 0:4788e1df7b55 361 // GET AND DISPLAY SPEED
rwcjoliver 0:4788e1df7b55 362 dashboard.getCurrentSpeed();
rwcjoliver 0:4788e1df7b55 363 remote.sendData(2, dashboard.currentSpeed); // Send speed to remote
rwcjoliver 0:4788e1df7b55 364
rwcjoliver 0:4788e1df7b55 365 // TOGGLE COMPRESSOR
rwcjoliver 0:4788e1df7b55 366 remote.compressor == 0 ? contactCompressor = 1 : contactCompressor = 0;
rwcjoliver 0:4788e1df7b55 367
rwcjoliver 0:4788e1df7b55 368 // TOGGLE MOTOR CONTROLLER DEADMAN (SEAT SWITCH AND FOOTBRAKE)
rwcjoliver 0:4788e1df7b55 369 if (rtc.deadman == 0) { // IF DEADMAN PRESSED
rwcjoliver 0:4788e1df7b55 370 motor1.closeDeadman();
rwcjoliver 0:4788e1df7b55 371 }
rwcjoliver 0:4788e1df7b55 372 else {
rwcjoliver 0:4788e1df7b55 373 motor1.releaseDeadman();
rwcjoliver 0:4788e1df7b55 374 }
rwcjoliver 0:4788e1df7b55 375
rwcjoliver 0:4788e1df7b55 376 // TOGGLE REGEN THROTTLING
rwcjoliver 0:4788e1df7b55 377 if (challenge.regenThrottleActive == false) {
rwcjoliver 0:4788e1df7b55 378 if (remote.regenThrottle == 0 && remote.start == 0) { // TURN OFF IF ON
rwcjoliver 0:4788e1df7b55 379 challenge.regenThrottleOn();
rwcjoliver 0:4788e1df7b55 380 }
rwcjoliver 0:4788e1df7b55 381 }
rwcjoliver 0:4788e1df7b55 382 else {
rwcjoliver 0:4788e1df7b55 383 remote.sendError('B'); // Send error to remote
rwcjoliver 0:4788e1df7b55 384 if (remote.regenThrottle == 1) { // TURN ON IF OFF
rwcjoliver 0:4788e1df7b55 385 challenge.regenThrottleOff();
rwcjoliver 0:4788e1df7b55 386 }
rwcjoliver 0:4788e1df7b55 387 }
rwcjoliver 0:4788e1df7b55 388
rwcjoliver 0:4788e1df7b55 389 // TOGGLE REGEN BRAKING
rwcjoliver 0:4788e1df7b55 390 if (challenge.regenBrakingActive == false) {
rwcjoliver 0:4788e1df7b55 391 if (remote.regenBrake == 0 && remote.start == 0) { // TURN OFF IF ON
rwcjoliver 0:4788e1df7b55 392 if (challenge.regenBrakingOn() == 0) {
rwcjoliver 0:4788e1df7b55 393 remote.sendError('I'); // Send error to remote
rwcjoliver 0:4788e1df7b55 394 pc.printf("Regen Braking Off - SuperCaps are full\r\n");
rwcjoliver 0:4788e1df7b55 395 }
rwcjoliver 0:4788e1df7b55 396 }
rwcjoliver 0:4788e1df7b55 397 }
rwcjoliver 0:4788e1df7b55 398 else {
rwcjoliver 0:4788e1df7b55 399 remote.sendError('C'); // Send error to remote
rwcjoliver 0:4788e1df7b55 400 if (remote.regenBrake == 1) { // TURN OFF
rwcjoliver 0:4788e1df7b55 401 challenge.regenBrakingOff();
rwcjoliver 0:4788e1df7b55 402 if (superCapVoltage == 1) {
rwcjoliver 0:4788e1df7b55 403 pc.printf("Regen Braking Off - SuperCaps are full\r\n");
rwcjoliver 0:4788e1df7b55 404 remote.sendError('I'); // Send error to remote
rwcjoliver 0:4788e1df7b55 405 }
rwcjoliver 0:4788e1df7b55 406 }
rwcjoliver 0:4788e1df7b55 407 }
rwcjoliver 0:4788e1df7b55 408
rwcjoliver 0:4788e1df7b55 409
rwcjoliver 0:4788e1df7b55 410 // TOGGLE AUTOSTOP
rwcjoliver 0:4788e1df7b55 411 if (challenge.autoStopActive == 0) {
rwcjoliver 0:4788e1df7b55 412 if (remote.autoStop == 0 && remote.start == 0) { // TURN OFF IF ON
rwcjoliver 0:4788e1df7b55 413 challenge.autoStopOn();
rwcjoliver 0:4788e1df7b55 414 }
rwcjoliver 0:4788e1df7b55 415 }
rwcjoliver 0:4788e1df7b55 416 else {
rwcjoliver 0:4788e1df7b55 417 remote.sendError('D'); // Send error to remote
rwcjoliver 0:4788e1df7b55 418 if (remote.autoStop == 1) { // TURN ON IF OFF
rwcjoliver 0:4788e1df7b55 419 challenge.autoStopOff();
rwcjoliver 0:4788e1df7b55 420 }
rwcjoliver 0:4788e1df7b55 421 }
rwcjoliver 0:4788e1df7b55 422
rwcjoliver 0:4788e1df7b55 423 // TOGGLE INNOVATION
rwcjoliver 0:4788e1df7b55 424 if (challenge.innovationActive == 0) {
rwcjoliver 0:4788e1df7b55 425 if (remote.innovation == 0 && remote.start == 0) { // TURN OFF IF ON
rwcjoliver 0:4788e1df7b55 426 if (driveMode == 0) {
rwcjoliver 0:4788e1df7b55 427 challenge.innovationOn();
rwcjoliver 0:4788e1df7b55 428 }
rwcjoliver 0:4788e1df7b55 429 else {
rwcjoliver 0:4788e1df7b55 430 remote.sendError('J'); // Send error to remote
rwcjoliver 0:4788e1df7b55 431 pc.printf("Can only active innovation mode in forward direction\r\n");
rwcjoliver 0:4788e1df7b55 432 }
rwcjoliver 0:4788e1df7b55 433 }
rwcjoliver 0:4788e1df7b55 434 }
rwcjoliver 0:4788e1df7b55 435 else {
rwcjoliver 0:4788e1df7b55 436 remote.sendError('E'); // Send error to remote
rwcjoliver 0:4788e1df7b55 437
rwcjoliver 0:4788e1df7b55 438 if (remote.innovation == 1) { // TURN ON IF OFF
rwcjoliver 0:4788e1df7b55 439 challenge.innovationOff();
rwcjoliver 0:4788e1df7b55 440 }
rwcjoliver 0:4788e1df7b55 441 }
rwcjoliver 0:4788e1df7b55 442
rwcjoliver 0:4788e1df7b55 443
rwcjoliver 0:4788e1df7b55 444 // CONTROL
rwcjoliver 0:4788e1df7b55 445
rwcjoliver 0:4788e1df7b55 446 if (systemOn == false) {
rwcjoliver 0:4788e1df7b55 447
rwcjoliver 0:4788e1df7b55 448 if (remote.start == 1) {
rwcjoliver 0:4788e1df7b55 449
rwcjoliver 0:4788e1df7b55 450 if (millis() - lastErrorMillis > 500) {
rwcjoliver 0:4788e1df7b55 451 remote.sendError('A'); // SEND ERROR MESSAGE 'A' TO REMOTE
rwcjoliver 0:4788e1df7b55 452 lastErrorMillis = millis();
rwcjoliver 0:4788e1df7b55 453 }
rwcjoliver 0:4788e1df7b55 454
rwcjoliver 0:4788e1df7b55 455 motor1.turnOff();
rwcjoliver 0:4788e1df7b55 456 // motor2.turnOff();
rwcjoliver 0:4788e1df7b55 457 }
rwcjoliver 0:4788e1df7b55 458 else {
rwcjoliver 0:4788e1df7b55 459 systemOn = true;
rwcjoliver 0:4788e1df7b55 460 pc.printf("Start Switch is On\r\n");
rwcjoliver 0:4788e1df7b55 461 // startupHealthCheck(); // Run System Startup Health Check - Will stay in here until it passes
rwcjoliver 0:4788e1df7b55 462
rwcjoliver 0:4788e1df7b55 463 motor1.turnOn(); // Turn on motors
rwcjoliver 0:4788e1df7b55 464 // motor2.turnOn();
rwcjoliver 0:4788e1df7b55 465
rwcjoliver 0:4788e1df7b55 466 }
rwcjoliver 0:4788e1df7b55 467 } // END IF SYSTEMON = FALSE
rwcjoliver 0:4788e1df7b55 468 else { // IF SYSTEMON == TRUE
rwcjoliver 0:4788e1df7b55 469 if (remote.start == 1) {
rwcjoliver 0:4788e1df7b55 470 systemOn = false; // WILL STOP ABOVE HERE NEXT LOOP
rwcjoliver 0:4788e1df7b55 471 pc.printf("Start Switch is Off\r\n");
rwcjoliver 0:4788e1df7b55 472 }
rwcjoliver 0:4788e1df7b55 473
rwcjoliver 0:4788e1df7b55 474 if (driveMode != 0 && remote.forward == 0) {
rwcjoliver 0:4788e1df7b55 475 driveMode = 0;
rwcjoliver 0:4788e1df7b55 476 motor1.setForward();
rwcjoliver 0:4788e1df7b55 477 // motor2.setForward();
rwcjoliver 0:4788e1df7b55 478 }
rwcjoliver 0:4788e1df7b55 479 if (driveMode != 1 && remote.reverse == 0) {
rwcjoliver 0:4788e1df7b55 480 driveMode = 1;
rwcjoliver 0:4788e1df7b55 481 motor1.setReverse();
rwcjoliver 0:4788e1df7b55 482 // motor2.setReverse();
rwcjoliver 0:4788e1df7b55 483 }
rwcjoliver 0:4788e1df7b55 484 if (driveMode != 2 && remote.park == 0) {
rwcjoliver 0:4788e1df7b55 485 driveMode = 2;
rwcjoliver 0:4788e1df7b55 486 motor1.setPark();
rwcjoliver 0:4788e1df7b55 487 motor1.throttle(0);
rwcjoliver 0:4788e1df7b55 488 // motor2.setPark();
rwcjoliver 0:4788e1df7b55 489 }
rwcjoliver 0:4788e1df7b55 490
rwcjoliver 0:4788e1df7b55 491 if (driveMode == 2) { //place in park mode if selected by driver
rwcjoliver 0:4788e1df7b55 492
rwcjoliver 0:4788e1df7b55 493 // pc.printf("RTC Output is %d\r\n", rtc_output.read());
rwcjoliver 0:4788e1df7b55 494 // pc.printf("EM State Output is %d\r\n", emergencyStopActive);
rwcjoliver 0:4788e1df7b55 495 // pc.printf("ParkMode = %d", inParkMode);
rwcjoliver 0:4788e1df7b55 496
rwcjoliver 0:4788e1df7b55 497 if (inParkMode == false) {
rwcjoliver 0:4788e1df7b55 498 pc.printf("Train in park mode.\r\n");
rwcjoliver 0:4788e1df7b55 499 }
rwcjoliver 0:4788e1df7b55 500
rwcjoliver 0:4788e1df7b55 501 if (emergencyStopActive == true && rtc_output.read() == 0) { // Clear emergency stop flag
rwcjoliver 0:4788e1df7b55 502 emergencyStopActive = false;
rwcjoliver 0:4788e1df7b55 503 }
rwcjoliver 0:4788e1df7b55 504
rwcjoliver 0:4788e1df7b55 505 led_parkMode = 1;
rwcjoliver 0:4788e1df7b55 506 inParkMode = true; // Stop above debug print from displaying more than once
rwcjoliver 0:4788e1df7b55 507
rwcjoliver 0:4788e1df7b55 508 motor1.setPark();
rwcjoliver 0:4788e1df7b55 509 // motor2.setPark();
rwcjoliver 0:4788e1df7b55 510
rwcjoliver 0:4788e1df7b55 511 }
rwcjoliver 0:4788e1df7b55 512 else{ //else i.e if selected drive mode is forward or reverse
rwcjoliver 0:4788e1df7b55 513 // pc.printf("RTC Output is %d\r\n", rtc_output.read());
rwcjoliver 0:4788e1df7b55 514 // pc.printf("EM State Output is %d\r\n", emergencyStopActive);
rwcjoliver 0:4788e1df7b55 515 if (emergencyStopActive == false && rtc_output.read() == 0) { // IF RTC FLAGGED AS SAFE
rwcjoliver 0:4788e1df7b55 516
rwcjoliver 0:4788e1df7b55 517 // if (dashboard.currentSpeed < 1 || driveMode == lastKnownDirection) { //do not allow motors to reverse if significant forward speed exists
rwcjoliver 0:4788e1df7b55 518
rwcjoliver 0:4788e1df7b55 519 led_parkMode = 0;
rwcjoliver 0:4788e1df7b55 520 inParkMode = false;
rwcjoliver 0:4788e1df7b55 521
rwcjoliver 0:4788e1df7b55 522 if (driveMode != lastKnownDirection) {
rwcjoliver 0:4788e1df7b55 523 pc.printf("Train enabled for direction %d\r\n", driveMode);
rwcjoliver 0:4788e1df7b55 524
rwcjoliver 0:4788e1df7b55 525 lastKnownDirection = driveMode;
rwcjoliver 0:4788e1df7b55 526 }
rwcjoliver 0:4788e1df7b55 527
rwcjoliver 0:4788e1df7b55 528
rwcjoliver 0:4788e1df7b55 529 if (challenge.autoStopInProgress == true) { // IF AUTOSTOPPING, PASS THROTTLE CONTROL TO FUNCTION
rwcjoliver 0:4788e1df7b55 530 challenge.autoStopControl();
rwcjoliver 0:4788e1df7b55 531 pc.printf("Autostop in Control\r\n");
rwcjoliver 0:4788e1df7b55 532 }
rwcjoliver 0:4788e1df7b55 533 else { // OTHERWISE INPUT THROTTLE FROM REMOTE
rwcjoliver 0:4788e1df7b55 534 if (remote.throttle > 0) { // If joystick pushed upwards = throttle
rwcjoliver 0:4788e1df7b55 535
rwcjoliver 0:4788e1df7b55 536 if (challenge.innovationActive == true) {
rwcjoliver 0:4788e1df7b55 537 pc.printf("Collision Detection in Control\r\n");
rwcjoliver 0:4788e1df7b55 538 int innovationThrottle = challenge.innovationControl(remote.throttle);
rwcjoliver 0:4788e1df7b55 539 speedControl(innovationThrottle);
rwcjoliver 0:4788e1df7b55 540
rwcjoliver 0:4788e1df7b55 541 if (innovationThrottle == 0) {
rwcjoliver 0:4788e1df7b55 542 emergencyStop(); // emergency Brake if obstacle detected
rwcjoliver 0:4788e1df7b55 543 }
rwcjoliver 0:4788e1df7b55 544 }
rwcjoliver 0:4788e1df7b55 545 else {
rwcjoliver 0:4788e1df7b55 546 speedControl(remote.throttle);
rwcjoliver 0:4788e1df7b55 547 pc.printf("Throttling: %d\r\n", remote.throttle);
rwcjoliver 0:4788e1df7b55 548 }
rwcjoliver 0:4788e1df7b55 549 } // remote.throttle
rwcjoliver 0:4788e1df7b55 550 else {
rwcjoliver 0:4788e1df7b55 551 speedControl(0);
rwcjoliver 0:4788e1df7b55 552 }
rwcjoliver 0:4788e1df7b55 553 }
rwcjoliver 0:4788e1df7b55 554
rwcjoliver 0:4788e1df7b55 555 // }
rwcjoliver 0:4788e1df7b55 556 // else {
rwcjoliver 0:4788e1df7b55 557 // pc.printf("Cannot change direction until train has stopped moving\r\n");
rwcjoliver 0:4788e1df7b55 558 // remote.sendError('F'); // Send error to remote
rwcjoliver 0:4788e1df7b55 559 // }
rwcjoliver 0:4788e1df7b55 560 }
rwcjoliver 0:4788e1df7b55 561 else {
rwcjoliver 0:4788e1df7b55 562 pc.printf("Cannot exit park mode until RTC is cleared\r\n");
rwcjoliver 0:4788e1df7b55 563 inParkMode = false;
rwcjoliver 0:4788e1df7b55 564 remote.sendError('G'); // Send error to remote
rwcjoliver 0:4788e1df7b55 565 }
rwcjoliver 0:4788e1df7b55 566 }
rwcjoliver 0:4788e1df7b55 567 } // END IF (SYSTEMON == TRUE)
rwcjoliver 0:4788e1df7b55 568
rwcjoliver 0:4788e1df7b55 569 wait_ms(500); // SLOW DOWN THE SYSTEM (REMOTE CANT KEEP UP)
rwcjoliver 0:4788e1df7b55 570 } // END WHILE(COMMSGOOD)
rwcjoliver 0:4788e1df7b55 571 pc.printf("Main Loop Skipped Due To Emergency Status\r\n");
rwcjoliver 0:4788e1df7b55 572 emergencyStop(); // Emergency stop if comms lost with remote controller
rwcjoliver 0:4788e1df7b55 573
rwcjoliver 0:4788e1df7b55 574 } //END WHILE(1)
rwcjoliver 0:4788e1df7b55 575 }