Railway Challenge / Mbed 2 deprecated challenge

Uncommenting of part that allow supercaps to charge up from the batteries

Dependencies:   mbed millis

main.cpp@2:d8d92dfc9c82, 2022-04-12 (annotated)

Committer:
jamesmcildowietfl
Date:
Tue Apr 12 16:46:46 2022 +0000
Revision:
2:d8d92dfc9c82
Parent:
1:ba85dae98035
Child:
3:32e951e05a5b
Changed Mechanical Braking to accomedate new brakes for testing.; Commented some lines starting from line ~157

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
jamesmcildowietfl 2:d8d92dfc9c82 141 brakeValve32 = 1;//(PF_2)
jamesmcildowietfl 2:d8d92dfc9c82 142 brakeValve22 = 1;//(PG_1)
rwcjoliver 0:4788e1df7b55 143 break;
jamesmcildowietfl 2:d8d92dfc9c82 144
jamesmcildowietfl 2:d8d92dfc9c82 145 case 1: //One brake high
jamesmcildowietfl 2:d8d92dfc9c82 146 motor1.throttle(0.0f);
jamesmcildowietfl 2:d8d92dfc9c82 147 brakeValve32 = 0;//(PF_2)
jamesmcildowietfl 2:d8d92dfc9c82 148 brakeValve22 = 1;//(PG_1)
jamesmcildowietfl 2:d8d92dfc9c82 149 break;
jamesmcildowietfl 2:d8d92dfc9c82 150 case 2 ... 4 : //Two brake high
rwcjoliver 0:4788e1df7b55 151 motor1.throttle(0.0f);
jamesmcildowietfl 2:d8d92dfc9c82 152 brakeValve32 = 0;//(PF_2)
jamesmcildowietfl 2:d8d92dfc9c82 153 brakeValve22 = 0;//(PG_1)
rwcjoliver 0:4788e1df7b55 154 break;
rwcjoliver 0:4788e1df7b55 155
jamesmcildowietfl 2:d8d92dfc9c82 156
jamesmcildowietfl 2:d8d92dfc9c82 157 // case 1:
jamesmcildowietfl 2:d8d92dfc9c82 158 // motor1.throttle(0.0f);
jamesmcildowietfl 2:d8d92dfc9c82 159 // brakeValve32 = 0;
jamesmcildowietfl 2:d8d92dfc9c82 160 // if (pressureSwitch1.read() == 0) {
jamesmcildowietfl 2:d8d92dfc9c82 161 // brakeValve22 = 0;
jamesmcildowietfl 2:d8d92dfc9c82 162 // pc.printf("Pressure 1 Reached");
jamesmcildowietfl 2:d8d92dfc9c82 163 // }
jamesmcildowietfl 2:d8d92dfc9c82 164 // else {
jamesmcildowietfl 2:d8d92dfc9c82 165 // brakeValve22 = 1;
jamesmcildowietfl 2:d8d92dfc9c82 166 // pc.printf("Braking Level 1\r\n");
jamesmcildowietfl 2:d8d92dfc9c82 167 // }
jamesmcildowietfl 2:d8d92dfc9c82 168 // break;
jamesmcildowietfl 2:d8d92dfc9c82 169
rwcjoliver 0:4788e1df7b55 170 // case 2:
rwcjoliver 0:4788e1df7b55 171 // motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 172 // brakeValve32 = 0;
rwcjoliver 0:4788e1df7b55 173 // if (pressureSwitch2.read() == 0) {
rwcjoliver 0:4788e1df7b55 174 // brakeValve22 = 0;
rwcjoliver 0:4788e1df7b55 175 // pc.printf("Pressure 2 Reached");
rwcjoliver 0:4788e1df7b55 176 // }
rwcjoliver 0:4788e1df7b55 177 // else {
rwcjoliver 0:4788e1df7b55 178 // brakeValve22 = 1;
rwcjoliver 0:4788e1df7b55 179 // pc.printf("Braking Level 2\r\n");
rwcjoliver 0:4788e1df7b55 180 // }
rwcjoliver 0:4788e1df7b55 181 //
rwcjoliver 0:4788e1df7b55 182 // break;
jamesmcildowietfl 2:d8d92dfc9c82 183 //
jamesmcildowietfl 2:d8d92dfc9c82 184 // case 2 ... 4:
jamesmcildowietfl 2:d8d92dfc9c82 185 // motor1.throttle(0.0f);
jamesmcildowietfl 2:d8d92dfc9c82 186 // brakeValve32 = 0;
jamesmcildowietfl 2:d8d92dfc9c82 187 // brakeValve22 = 1;
rwcjoliver 0:4788e1df7b55 188
rwcjoliver 0:4788e1df7b55 189 // if (pressureSwitch3.read() == 0) {
rwcjoliver 0:4788e1df7b55 190 // brakeValve22 = 0;
rwcjoliver 0:4788e1df7b55 191 // pc.printf("Pressure 3 Reached");
rwcjoliver 0:4788e1df7b55 192 // }
rwcjoliver 0:4788e1df7b55 193 // else {
rwcjoliver 0:4788e1df7b55 194 // brakeValve22 = 1;
rwcjoliver 0:4788e1df7b55 195 // pc.printf("Braking Level 3\r\n");
rwcjoliver 0:4788e1df7b55 196 // }
jamesmcildowietfl 2:d8d92dfc9c82 197 // break;
rwcjoliver 0:4788e1df7b55 198
rwcjoliver 0:4788e1df7b55 199
rwcjoliver 0:4788e1df7b55 200 default: // NO BRAKING
rwcjoliver 0:4788e1df7b55 201 brakeValve32 = 1;
rwcjoliver 0:4788e1df7b55 202 brakeValve22 = 1;
rwcjoliver 0:4788e1df7b55 203 break;
rwcjoliver 0:4788e1df7b55 204 }
rwcjoliver 0:4788e1df7b55 205 }
rwcjoliver 0:4788e1df7b55 206 }
rwcjoliver 0:4788e1df7b55 207 return;
rwcjoliver 0:4788e1df7b55 208 }
rwcjoliver 0:4788e1df7b55 209
rwcjoliver 0:4788e1df7b55 210 void speedControl(int commandedSpeed) {
rwcjoliver 0:4788e1df7b55 211 if (dashboard.currentSpeed < 16.00) { // IF SPEED LESS THAN LIMIT
rwcjoliver 0:4788e1df7b55 212 switch (commandedSpeed) {
rwcjoliver 0:4788e1df7b55 213
rwcjoliver 0:4788e1df7b55 214 default:
rwcjoliver 0:4788e1df7b55 215 // motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 216 break;
rwcjoliver 0:4788e1df7b55 217
rwcjoliver 0:4788e1df7b55 218 case 0:
rwcjoliver 0:4788e1df7b55 219 motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 220 break;
rwcjoliver 0:4788e1df7b55 221
rwcjoliver 0:4788e1df7b55 222 case 1 ... 2:
rwcjoliver 0:4788e1df7b55 223 motor1.throttle(0.1f);
rwcjoliver 0:4788e1df7b55 224 break;
rwcjoliver 0:4788e1df7b55 225
rwcjoliver 0:4788e1df7b55 226 case 3 ... 4:
rwcjoliver 0:4788e1df7b55 227 motor1.throttle(0.2f);
rwcjoliver 0:4788e1df7b55 228 break;
rwcjoliver 0:4788e1df7b55 229
rwcjoliver 0:4788e1df7b55 230 case 5 ... 6:
rwcjoliver 0:4788e1df7b55 231 motor1.throttle(0.3f);
rwcjoliver 0:4788e1df7b55 232 break;
rwcjoliver 0:4788e1df7b55 233
rwcjoliver 0:4788e1df7b55 234 case 7 ... 8:
rwcjoliver 0:4788e1df7b55 235 motor1.throttle(0.4f);
rwcjoliver 0:4788e1df7b55 236 break;
rwcjoliver 0:4788e1df7b55 237
rwcjoliver 0:4788e1df7b55 238 case 9 ... 10:
rwcjoliver 0:4788e1df7b55 239 motor1.throttle(0.5f);
rwcjoliver 0:4788e1df7b55 240 break;
rwcjoliver 0:4788e1df7b55 241
rwcjoliver 0:4788e1df7b55 242 case 11:
rwcjoliver 0:4788e1df7b55 243 motor1.throttle(0.6f);
rwcjoliver 0:4788e1df7b55 244 break;
rwcjoliver 0:4788e1df7b55 245
rwcjoliver 0:4788e1df7b55 246 case 12:
rwcjoliver 0:4788e1df7b55 247 motor1.throttle(0.7f);
rwcjoliver 0:4788e1df7b55 248 break;
rwcjoliver 0:4788e1df7b55 249
rwcjoliver 0:4788e1df7b55 250 case 13:
rwcjoliver 0:4788e1df7b55 251 motor1.throttle(0.8f);
rwcjoliver 0:4788e1df7b55 252 break;
rwcjoliver 0:4788e1df7b55 253
rwcjoliver 0:4788e1df7b55 254 case 14:
rwcjoliver 0:4788e1df7b55 255 motor1.throttle(0.9f);
rwcjoliver 0:4788e1df7b55 256 break;
rwcjoliver 0:4788e1df7b55 257
rwcjoliver 0:4788e1df7b55 258 case 15:
rwcjoliver 0:4788e1df7b55 259 motor1.throttle(1.0f);
rwcjoliver 0:4788e1df7b55 260 break;
rwcjoliver 0:4788e1df7b55 261 }
rwcjoliver 0:4788e1df7b55 262 }
rwcjoliver 0:4788e1df7b55 263 else { // IF OVER 15KPH
rwcjoliver 0:4788e1df7b55 264 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 265 motor1.throttle(0.0f); // COMMENTED AS ALREADY SET 0 IN BRAKECONTROL
rwcjoliver 0:4788e1df7b55 266 // brakeControl(1);
rwcjoliver 0:4788e1df7b55 267 }
rwcjoliver 0:4788e1df7b55 268 }
rwcjoliver 0:4788e1df7b55 269 }
rwcjoliver 0:4788e1df7b55 270
rwcjoliver 0:4788e1df7b55 271 /*void speedControl(int commandedSpeed) {
rwcjoliver 0:4788e1df7b55 272 if (commandedSpeed > dashboard.currentSpeed) { // IF THROTTLE REQUESTED
rwcjoliver 0:4788e1df7b55 273 // Max possible difference is 15
rwcjoliver 0:4788e1df7b55 274 // Motor Analog Voltage between 0 and 5
rwcjoliver 0:4788e1df7b55 275 // 5 / 15 = 0.33333 = 0.4v / kph difference
rwcjoliver 0:4788e1df7b55 276
rwcjoliver 0:4788e1df7b55 277 int difference = commandedSpeed - dashboard.currentSpeed;
rwcjoliver 0:4788e1df7b55 278
rwcjoliver 0:4788e1df7b55 279 switch (difference) {
rwcjoliver 0:4788e1df7b55 280 case 1:
rwcjoliver 0:4788e1df7b55 281 motor1.throttle(0.1f);
rwcjoliver 0:4788e1df7b55 282 // motor2.throttle(0.1f);
rwcjoliver 0:4788e1df7b55 283 pc.printf("Throttle set to 10%\r\n");
rwcjoliver 0:4788e1df7b55 284 break;
rwcjoliver 0:4788e1df7b55 285
rwcjoliver 0:4788e1df7b55 286 case 2 ... 3:
rwcjoliver 0:4788e1df7b55 287 motor1.throttle(0.2f);
rwcjoliver 0:4788e1df7b55 288 // motor2.throttle(0.2f);
rwcjoliver 0:4788e1df7b55 289 pc.printf("Throttle set to 20%\r\n");
rwcjoliver 0:4788e1df7b55 290 break;
rwcjoliver 0:4788e1df7b55 291
rwcjoliver 0:4788e1df7b55 292 case 4 ... 6:
rwcjoliver 0:4788e1df7b55 293 motor1.throttle(0.4f);
rwcjoliver 0:4788e1df7b55 294 // motor2.throttle(0.4f);
rwcjoliver 0:4788e1df7b55 295 pc.printf("Throttle set to 40%\r\n");
rwcjoliver 0:4788e1df7b55 296 break;
rwcjoliver 0:4788e1df7b55 297
rwcjoliver 0:4788e1df7b55 298 case 7 ... 9:
rwcjoliver 0:4788e1df7b55 299 motor1.throttle(0.6f);
rwcjoliver 0:4788e1df7b55 300 // motor2.throttle(0.6f);
rwcjoliver 0:4788e1df7b55 301 pc.printf("Throttle set to 60%\r\n");
rwcjoliver 0:4788e1df7b55 302 break;
rwcjoliver 0:4788e1df7b55 303
rwcjoliver 0:4788e1df7b55 304 case 10 ... 12:
rwcjoliver 0:4788e1df7b55 305 motor1.throttle(0.8f);
rwcjoliver 0:4788e1df7b55 306 // motor2.throttle(0.8f);
rwcjoliver 0:4788e1df7b55 307 pc.printf("Throttle set to 80%\r\n");
rwcjoliver 0:4788e1df7b55 308 break;
rwcjoliver 0:4788e1df7b55 309
rwcjoliver 0:4788e1df7b55 310 case 13 ... 15:
rwcjoliver 0:4788e1df7b55 311 motor1.throttle(1.0f);
rwcjoliver 0:4788e1df7b55 312 // motor2.throttle(1.0f);
rwcjoliver 0:4788e1df7b55 313 pc.printf("Throttle set to 100%\r\n");
rwcjoliver 0:4788e1df7b55 314 break;
rwcjoliver 0:4788e1df7b55 315
rwcjoliver 0:4788e1df7b55 316 default:
rwcjoliver 0:4788e1df7b55 317 motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 318 break;
rwcjoliver 0:4788e1df7b55 319 }
rwcjoliver 0:4788e1df7b55 320 }
rwcjoliver 0:4788e1df7b55 321 else { // COAST
rwcjoliver 0:4788e1df7b55 322 motor1.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 323 // motor2.throttle(0.0f);
rwcjoliver 0:4788e1df7b55 324 }
rwcjoliver 0:4788e1df7b55 325 }*/
rwcjoliver 0:4788e1df7b55 326
rwcjoliver 0:4788e1df7b55 327 int main() {
rwcjoliver 0:4788e1df7b55 328 pc.baud(115200);
rwcjoliver 0:4788e1df7b55 329
rwcjoliver 0:4788e1df7b55 330 // CONFIGURE INTERRUPTS
rwcjoliver 0:4788e1df7b55 331 rtc_output.rise(&emergencyStop);
rwcjoliver 0:4788e1df7b55 332
rwcjoliver 0:4788e1df7b55 333 millisStart();
rwcjoliver 0:4788e1df7b55 334
rwcjoliver 0:4788e1df7b55 335 rtc_Trigger = 1;
rwcjoliver 0:4788e1df7b55 336
rwcjoliver 0:4788e1df7b55 337 // LOCAL VARIABLES
rwcjoliver 0:4788e1df7b55 338 bool systemOn = false; // On/Off status of loco
rwcjoliver 0:4788e1df7b55 339 int lastKnownDirection = 2;
rwcjoliver 0:4788e1df7b55 340 bool inParkMode = false;
rwcjoliver 0:4788e1df7b55 341
rwcjoliver 0:4788e1df7b55 342 // Record last time error was sent
rwcjoliver 0:4788e1df7b55 343 unsigned long lastErrorMillis = 0;
rwcjoliver 0:4788e1df7b55 344
rwcjoliver 0:4788e1df7b55 345 while(1) {
rwcjoliver 0:4788e1df7b55 346
rwcjoliver 0:4788e1df7b55 347 while(remote.commsGood == true) {
rwcjoliver 0:4788e1df7b55 348
rwcjoliver 0:4788e1df7b55 349 // PING
rwcjoliver 0:4788e1df7b55 350 remote.commsCheck();
rwcjoliver 0:4788e1df7b55 351 // GET SWITCH STATES
rwcjoliver 0:4788e1df7b55 352 remote.getSwitchStates();
rwcjoliver 0:4788e1df7b55 353
rwcjoliver 0:4788e1df7b55 354 // ALLOW BRAKES TO BE OPERATED
rwcjoliver 0:4788e1df7b55 355 brakeControl(remote.braking);
rwcjoliver 0:4788e1df7b55 356
rwcjoliver 0:4788e1df7b55 357 // Print Pressure Switch States (Debugging)
rwcjoliver 0:4788e1df7b55 358 // pc.printf("Pressure Switch 1: %d\r\n", pressureSwitch1.read());
rwcjoliver 0:4788e1df7b55 359 // pc.printf("Pressure Switch 2: %d\r\n", pressureSwitch2.read());
rwcjoliver 0:4788e1df7b55 360 // pc.printf("Pressure Switch 3: %d\r\n", pressureSwitch3.read());
rwcjoliver 0:4788e1df7b55 361
rwcjoliver 0:4788e1df7b55 362 // SOUND WHISTLE IF WHISTLE BUTTON PRESSED
rwcjoliver 0:4788e1df7b55 363 if (remote.whistle == 0) {
rwcjoliver 0:4788e1df7b55 364 whistleValve32 = 1;
rwcjoliver 0:4788e1df7b55 365 // wait(0.5);
rwcjoliver 0:4788e1df7b55 366 // whistleValve32 = 1;
rwcjoliver 0:4788e1df7b55 367 }
rwcjoliver 0:4788e1df7b55 368 else {
rwcjoliver 0:4788e1df7b55 369 whistleValve32 = 0;
rwcjoliver 0:4788e1df7b55 370 }
rwcjoliver 0:4788e1df7b55 371
rwcjoliver 0:4788e1df7b55 372
rwcjoliver 0:4788e1df7b55 373 // GET AND DISPLAY SPEED
rwcjoliver 0:4788e1df7b55 374 dashboard.getCurrentSpeed();
rwcjoliver 0:4788e1df7b55 375 remote.sendData(2, dashboard.currentSpeed); // Send speed to remote
rwcjoliver 0:4788e1df7b55 376
rwcjoliver 0:4788e1df7b55 377 // TOGGLE COMPRESSOR
rwcjoliver 0:4788e1df7b55 378 remote.compressor == 0 ? contactCompressor = 1 : contactCompressor = 0;
rwcjoliver 0:4788e1df7b55 379
rwcjoliver 0:4788e1df7b55 380 // TOGGLE MOTOR CONTROLLER DEADMAN (SEAT SWITCH AND FOOTBRAKE)
rwcjoliver 0:4788e1df7b55 381 if (rtc.deadman == 0) { // IF DEADMAN PRESSED
rwcjoliver 0:4788e1df7b55 382 motor1.closeDeadman();
rwcjoliver 0:4788e1df7b55 383 }
rwcjoliver 0:4788e1df7b55 384 else {
rwcjoliver 0:4788e1df7b55 385 motor1.releaseDeadman();
rwcjoliver 0:4788e1df7b55 386 }
rwcjoliver 0:4788e1df7b55 387
rwcjoliver 0:4788e1df7b55 388 // TOGGLE REGEN THROTTLING
rwcjoliver 0:4788e1df7b55 389 if (challenge.regenThrottleActive == false) {
rwcjoliver 0:4788e1df7b55 390 if (remote.regenThrottle == 0 && remote.start == 0) { // TURN OFF IF ON
rwcjoliver 0:4788e1df7b55 391 challenge.regenThrottleOn();
rwcjoliver 0:4788e1df7b55 392 }
rwcjoliver 0:4788e1df7b55 393 }
rwcjoliver 0:4788e1df7b55 394 else {
rwcjoliver 0:4788e1df7b55 395 remote.sendError('B'); // Send error to remote
rwcjoliver 0:4788e1df7b55 396 if (remote.regenThrottle == 1) { // TURN ON IF OFF
rwcjoliver 0:4788e1df7b55 397 challenge.regenThrottleOff();
rwcjoliver 0:4788e1df7b55 398 }
rwcjoliver 0:4788e1df7b55 399 }
rwcjoliver 0:4788e1df7b55 400
rwcjoliver 0:4788e1df7b55 401 // TOGGLE REGEN BRAKING
rwcjoliver 0:4788e1df7b55 402 if (challenge.regenBrakingActive == false) {
rwcjoliver 0:4788e1df7b55 403 if (remote.regenBrake == 0 && remote.start == 0) { // TURN OFF IF ON
rwcjoliver 0:4788e1df7b55 404 if (challenge.regenBrakingOn() == 0) {
rwcjoliver 0:4788e1df7b55 405 remote.sendError('I'); // Send error to remote
rwcjoliver 0:4788e1df7b55 406 pc.printf("Regen Braking Off - SuperCaps are full\r\n");
rwcjoliver 0:4788e1df7b55 407 }
rwcjoliver 0:4788e1df7b55 408 }
rwcjoliver 0:4788e1df7b55 409 }
rwcjoliver 0:4788e1df7b55 410 else {
rwcjoliver 0:4788e1df7b55 411 remote.sendError('C'); // Send error to remote
rwcjoliver 0:4788e1df7b55 412 if (remote.regenBrake == 1) { // TURN OFF
rwcjoliver 0:4788e1df7b55 413 challenge.regenBrakingOff();
rwcjoliver 0:4788e1df7b55 414 if (superCapVoltage == 1) {
rwcjoliver 0:4788e1df7b55 415 pc.printf("Regen Braking Off - SuperCaps are full\r\n");
rwcjoliver 0:4788e1df7b55 416 remote.sendError('I'); // Send error to remote
rwcjoliver 0:4788e1df7b55 417 }
rwcjoliver 0:4788e1df7b55 418 }
rwcjoliver 0:4788e1df7b55 419 }
rwcjoliver 0:4788e1df7b55 420
rwcjoliver 0:4788e1df7b55 421 // TOGGLE AUTOSTOP
rwcjoliver 0:4788e1df7b55 422 if (challenge.autoStopActive == 0) {
rwcjoliver 0:4788e1df7b55 423 if (remote.autoStop == 0 && remote.start == 0) { // TURN OFF IF ON
rwcjoliver 0:4788e1df7b55 424 challenge.autoStopOn();
rwcjoliver 0:4788e1df7b55 425 }
rwcjoliver 0:4788e1df7b55 426 }
rwcjoliver 0:4788e1df7b55 427 else {
rwcjoliver 0:4788e1df7b55 428 remote.sendError('D'); // Send error to remote
rwcjoliver 0:4788e1df7b55 429 if (remote.autoStop == 1) { // TURN ON IF OFF
rwcjoliver 0:4788e1df7b55 430 challenge.autoStopOff();
rwcjoliver 0:4788e1df7b55 431 }
rwcjoliver 0:4788e1df7b55 432 }
rwcjoliver 0:4788e1df7b55 433
rwcjoliver 0:4788e1df7b55 434 // TOGGLE INNOVATION
rwcjoliver 0:4788e1df7b55 435 if (challenge.innovationActive == 0) {
rwcjoliver 0:4788e1df7b55 436 if (remote.innovation == 0 && remote.start == 0) { // TURN OFF IF ON
rwcjoliver 0:4788e1df7b55 437 if (driveMode == 0) {
rwcjoliver 0:4788e1df7b55 438 challenge.innovationOn();
rwcjoliver 0:4788e1df7b55 439 }
rwcjoliver 0:4788e1df7b55 440 else {
rwcjoliver 0:4788e1df7b55 441 remote.sendError('J'); // Send error to remote
rwcjoliver 0:4788e1df7b55 442 pc.printf("Can only active innovation mode in forward direction\r\n");
rwcjoliver 0:4788e1df7b55 443 }
rwcjoliver 0:4788e1df7b55 444 }
rwcjoliver 0:4788e1df7b55 445 }
rwcjoliver 0:4788e1df7b55 446 else {
rwcjoliver 0:4788e1df7b55 447 remote.sendError('E'); // Send error to remote
rwcjoliver 0:4788e1df7b55 448
rwcjoliver 0:4788e1df7b55 449 if (remote.innovation == 1) { // TURN ON IF OFF
rwcjoliver 0:4788e1df7b55 450 challenge.innovationOff();
rwcjoliver 0:4788e1df7b55 451 }
rwcjoliver 0:4788e1df7b55 452 }
rwcjoliver 0:4788e1df7b55 453
rwcjoliver 0:4788e1df7b55 454
rwcjoliver 0:4788e1df7b55 455 // CONTROL
rwcjoliver 0:4788e1df7b55 456
rwcjoliver 0:4788e1df7b55 457 if (systemOn == false) {
rwcjoliver 0:4788e1df7b55 458
rwcjoliver 0:4788e1df7b55 459 if (remote.start == 1) {
rwcjoliver 0:4788e1df7b55 460
rwcjoliver 0:4788e1df7b55 461 if (millis() - lastErrorMillis > 500) {
rwcjoliver 0:4788e1df7b55 462 remote.sendError('A'); // SEND ERROR MESSAGE 'A' TO REMOTE
rwcjoliver 0:4788e1df7b55 463 lastErrorMillis = millis();
rwcjoliver 0:4788e1df7b55 464 }
rwcjoliver 0:4788e1df7b55 465
rwcjoliver 0:4788e1df7b55 466 motor1.turnOff();
rwcjoliver 0:4788e1df7b55 467 // motor2.turnOff();
rwcjoliver 0:4788e1df7b55 468 }
rwcjoliver 0:4788e1df7b55 469 else {
rwcjoliver 0:4788e1df7b55 470 systemOn = true;
rwcjoliver 0:4788e1df7b55 471 pc.printf("Start Switch is On\r\n");
rwcjoliver 0:4788e1df7b55 472 // startupHealthCheck(); // Run System Startup Health Check - Will stay in here until it passes
rwcjoliver 0:4788e1df7b55 473
rwcjoliver 0:4788e1df7b55 474 motor1.turnOn(); // Turn on motors
rwcjoliver 0:4788e1df7b55 475 // motor2.turnOn();
rwcjoliver 0:4788e1df7b55 476
rwcjoliver 0:4788e1df7b55 477 }
rwcjoliver 0:4788e1df7b55 478 } // END IF SYSTEMON = FALSE
rwcjoliver 0:4788e1df7b55 479 else { // IF SYSTEMON == TRUE
rwcjoliver 0:4788e1df7b55 480 if (remote.start == 1) {
rwcjoliver 0:4788e1df7b55 481 systemOn = false; // WILL STOP ABOVE HERE NEXT LOOP
rwcjoliver 0:4788e1df7b55 482 pc.printf("Start Switch is Off\r\n");
rwcjoliver 0:4788e1df7b55 483 }
rwcjoliver 0:4788e1df7b55 484
rwcjoliver 0:4788e1df7b55 485 if (driveMode != 0 && remote.forward == 0) {
rwcjoliver 0:4788e1df7b55 486 driveMode = 0;
rwcjoliver 0:4788e1df7b55 487 motor1.setForward();
rwcjoliver 0:4788e1df7b55 488 // motor2.setForward();
rwcjoliver 0:4788e1df7b55 489 }
rwcjoliver 0:4788e1df7b55 490 if (driveMode != 1 && remote.reverse == 0) {
rwcjoliver 0:4788e1df7b55 491 driveMode = 1;
rwcjoliver 0:4788e1df7b55 492 motor1.setReverse();
rwcjoliver 0:4788e1df7b55 493 // motor2.setReverse();
rwcjoliver 0:4788e1df7b55 494 }
rwcjoliver 0:4788e1df7b55 495 if (driveMode != 2 && remote.park == 0) {
rwcjoliver 0:4788e1df7b55 496 driveMode = 2;
rwcjoliver 0:4788e1df7b55 497 motor1.setPark();
rwcjoliver 0:4788e1df7b55 498 motor1.throttle(0);
rwcjoliver 0:4788e1df7b55 499 // motor2.setPark();
rwcjoliver 0:4788e1df7b55 500 }
rwcjoliver 0:4788e1df7b55 501
rwcjoliver 0:4788e1df7b55 502 if (driveMode == 2) { //place in park mode if selected by driver
rwcjoliver 0:4788e1df7b55 503
rwcjoliver 0:4788e1df7b55 504 // pc.printf("RTC Output is %d\r\n", rtc_output.read());
rwcjoliver 0:4788e1df7b55 505 // pc.printf("EM State Output is %d\r\n", emergencyStopActive);
rwcjoliver 0:4788e1df7b55 506 // pc.printf("ParkMode = %d", inParkMode);
rwcjoliver 0:4788e1df7b55 507
rwcjoliver 0:4788e1df7b55 508 if (inParkMode == false) {
rwcjoliver 0:4788e1df7b55 509 pc.printf("Train in park mode.\r\n");
rwcjoliver 0:4788e1df7b55 510 }
rwcjoliver 0:4788e1df7b55 511
rwcjoliver 0:4788e1df7b55 512 if (emergencyStopActive == true && rtc_output.read() == 0) { // Clear emergency stop flag
rwcjoliver 0:4788e1df7b55 513 emergencyStopActive = false;
rwcjoliver 0:4788e1df7b55 514 }
rwcjoliver 0:4788e1df7b55 515
rwcjoliver 0:4788e1df7b55 516 led_parkMode = 1;
rwcjoliver 0:4788e1df7b55 517 inParkMode = true; // Stop above debug print from displaying more than once
rwcjoliver 0:4788e1df7b55 518
rwcjoliver 0:4788e1df7b55 519 motor1.setPark();
rwcjoliver 0:4788e1df7b55 520 // motor2.setPark();
rwcjoliver 0:4788e1df7b55 521
rwcjoliver 0:4788e1df7b55 522 }
rwcjoliver 0:4788e1df7b55 523 else{ //else i.e if selected drive mode is forward or reverse
rwcjoliver 0:4788e1df7b55 524 // pc.printf("RTC Output is %d\r\n", rtc_output.read());
rwcjoliver 0:4788e1df7b55 525 // pc.printf("EM State Output is %d\r\n", emergencyStopActive);
rwcjoliver 0:4788e1df7b55 526 if (emergencyStopActive == false && rtc_output.read() == 0) { // IF RTC FLAGGED AS SAFE
rwcjoliver 0:4788e1df7b55 527
rwcjoliver 0:4788e1df7b55 528 // if (dashboard.currentSpeed < 1 || driveMode == lastKnownDirection) { //do not allow motors to reverse if significant forward speed exists
rwcjoliver 0:4788e1df7b55 529
rwcjoliver 0:4788e1df7b55 530 led_parkMode = 0;
rwcjoliver 0:4788e1df7b55 531 inParkMode = false;
rwcjoliver 0:4788e1df7b55 532
rwcjoliver 0:4788e1df7b55 533 if (driveMode != lastKnownDirection) {
rwcjoliver 0:4788e1df7b55 534 pc.printf("Train enabled for direction %d\r\n", driveMode);
rwcjoliver 0:4788e1df7b55 535
rwcjoliver 0:4788e1df7b55 536 lastKnownDirection = driveMode;
rwcjoliver 0:4788e1df7b55 537 }
rwcjoliver 0:4788e1df7b55 538
rwcjoliver 0:4788e1df7b55 539
rwcjoliver 0:4788e1df7b55 540 if (challenge.autoStopInProgress == true) { // IF AUTOSTOPPING, PASS THROTTLE CONTROL TO FUNCTION
rwcjoliver 0:4788e1df7b55 541 challenge.autoStopControl();
rwcjoliver 0:4788e1df7b55 542 pc.printf("Autostop in Control\r\n");
rwcjoliver 0:4788e1df7b55 543 }
rwcjoliver 0:4788e1df7b55 544 else { // OTHERWISE INPUT THROTTLE FROM REMOTE
rwcjoliver 0:4788e1df7b55 545 if (remote.throttle > 0) { // If joystick pushed upwards = throttle
rwcjoliver 0:4788e1df7b55 546
rwcjoliver 0:4788e1df7b55 547 if (challenge.innovationActive == true) {
rwcjoliver 0:4788e1df7b55 548 pc.printf("Collision Detection in Control\r\n");
rwcjoliver 0:4788e1df7b55 549 int innovationThrottle = challenge.innovationControl(remote.throttle);
rwcjoliver 0:4788e1df7b55 550 speedControl(innovationThrottle);
rwcjoliver 0:4788e1df7b55 551
rwcjoliver 0:4788e1df7b55 552 if (innovationThrottle == 0) {
rwcjoliver 0:4788e1df7b55 553 emergencyStop(); // emergency Brake if obstacle detected
rwcjoliver 0:4788e1df7b55 554 }
rwcjoliver 0:4788e1df7b55 555 }
rwcjoliver 0:4788e1df7b55 556 else {
rwcjoliver 0:4788e1df7b55 557 speedControl(remote.throttle);
rwcjoliver 0:4788e1df7b55 558 pc.printf("Throttling: %d\r\n", remote.throttle);
rwcjoliver 0:4788e1df7b55 559 }
rwcjoliver 0:4788e1df7b55 560 } // remote.throttle
rwcjoliver 0:4788e1df7b55 561 else {
rwcjoliver 0:4788e1df7b55 562 speedControl(0);
rwcjoliver 0:4788e1df7b55 563 }
rwcjoliver 0:4788e1df7b55 564 }
rwcjoliver 0:4788e1df7b55 565
rwcjoliver 0:4788e1df7b55 566 // }
rwcjoliver 0:4788e1df7b55 567 // else {
rwcjoliver 0:4788e1df7b55 568 // pc.printf("Cannot change direction until train has stopped moving\r\n");
rwcjoliver 0:4788e1df7b55 569 // remote.sendError('F'); // Send error to remote
rwcjoliver 0:4788e1df7b55 570 // }
rwcjoliver 0:4788e1df7b55 571 }
rwcjoliver 0:4788e1df7b55 572 else {
rwcjoliver 0:4788e1df7b55 573 pc.printf("Cannot exit park mode until RTC is cleared\r\n");
rwcjoliver 0:4788e1df7b55 574 inParkMode = false;
rwcjoliver 0:4788e1df7b55 575 remote.sendError('G'); // Send error to remote
rwcjoliver 0:4788e1df7b55 576 }
rwcjoliver 0:4788e1df7b55 577 }
rwcjoliver 0:4788e1df7b55 578 } // END IF (SYSTEMON == TRUE)
rwcjoliver 0:4788e1df7b55 579
rwcjoliver 0:4788e1df7b55 580 wait_ms(500); // SLOW DOWN THE SYSTEM (REMOTE CANT KEEP UP)
rwcjoliver 0:4788e1df7b55 581 } // END WHILE(COMMSGOOD)
rwcjoliver 0:4788e1df7b55 582 pc.printf("Main Loop Skipped Due To Emergency Status\r\n");
rwcjoliver 0:4788e1df7b55 583 emergencyStop(); // Emergency stop if comms lost with remote controller
rwcjoliver 0:4788e1df7b55 584
rwcjoliver 0:4788e1df7b55 585 } //END WHILE(1)
rwcjoliver 0:4788e1df7b55 586 }