Railway Challenge / Mbed 2 deprecated challenge

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

Dependencies:   mbed millis

main.cpp@4:d9279fc3a8fb, 2022-04-13 (annotated)

Committer:
cdevarakonda
Date:
Wed Apr 13 08:42:27 2022 +0000
Revision:
4:d9279fc3a8fb
Parent:
3:32e951e05a5b
Child:
5:1911475688a8
Updated display function invoking to under comms good check (line 358)

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