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@6:d0ba4c659e9c, 2022-04-13 (annotated)

Committer:
cdevarakonda
Date:
Wed Apr 13 12:39:00 2022 +0000
Revision:
6:d0ba4c659e9c
Parent:
5:1911475688a8
Child:
8:0fe9f7bde2f9
Added braking in emergency stop function and edited out redundancy in setPArk

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