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@3:32e951e05a5b, 2022-04-12 (annotated)

Committer:
jamesmcildowietfl
Date:
Tue Apr 12 17:19:05 2022 +0000
Revision:
3:32e951e05a5b
Parent:
2:d8d92dfc9c82
Child:
4:d9279fc3a8fb
Added Chai's code for data logger output, see line 10, 11 and the new function DisplaySerial, which is included at the start of the main loop

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) {
jamesmcildowietfl 3:32e951e05a5b 356 DisplaySerial();//Datalogger Chai Funciton
rwcjoliver 0:4788e1df7b55 357
rwcjoliver 0:4788e1df7b55 358 while(remote.commsGood == true) {
rwcjoliver 0:4788e1df7b55 359
rwcjoliver 0:4788e1df7b55 360 // PING
rwcjoliver 0:4788e1df7b55 361 remote.commsCheck();
rwcjoliver 0:4788e1df7b55 362 // GET SWITCH STATES
rwcjoliver 0:4788e1df7b55 363 remote.getSwitchStates();
rwcjoliver 0:4788e1df7b55 364
rwcjoliver 0:4788e1df7b55 365 // ALLOW BRAKES TO BE OPERATED
rwcjoliver 0:4788e1df7b55 366 brakeControl(remote.braking);
rwcjoliver 0:4788e1df7b55 367
rwcjoliver 0:4788e1df7b55 368 // Print Pressure Switch States (Debugging)
rwcjoliver 0:4788e1df7b55 369 // pc.printf("Pressure Switch 1: %d\r\n", pressureSwitch1.read());
rwcjoliver 0:4788e1df7b55 370 // pc.printf("Pressure Switch 2: %d\r\n", pressureSwitch2.read());
rwcjoliver 0:4788e1df7b55 371 // pc.printf("Pressure Switch 3: %d\r\n", pressureSwitch3.read());
rwcjoliver 0:4788e1df7b55 372
rwcjoliver 0:4788e1df7b55 373 // SOUND WHISTLE IF WHISTLE BUTTON PRESSED
rwcjoliver 0:4788e1df7b55 374 if (remote.whistle == 0) {
rwcjoliver 0:4788e1df7b55 375 whistleValve32 = 1;
rwcjoliver 0:4788e1df7b55 376 // wait(0.5);
rwcjoliver 0:4788e1df7b55 377 // whistleValve32 = 1;
rwcjoliver 0:4788e1df7b55 378 }
rwcjoliver 0:4788e1df7b55 379 else {
rwcjoliver 0:4788e1df7b55 380 whistleValve32 = 0;
rwcjoliver 0:4788e1df7b55 381 }
rwcjoliver 0:4788e1df7b55 382
rwcjoliver 0:4788e1df7b55 383
rwcjoliver 0:4788e1df7b55 384 // GET AND DISPLAY SPEED
rwcjoliver 0:4788e1df7b55 385 dashboard.getCurrentSpeed();
rwcjoliver 0:4788e1df7b55 386 remote.sendData(2, dashboard.currentSpeed); // Send speed to remote
rwcjoliver 0:4788e1df7b55 387
rwcjoliver 0:4788e1df7b55 388 // TOGGLE COMPRESSOR
rwcjoliver 0:4788e1df7b55 389 remote.compressor == 0 ? contactCompressor = 1 : contactCompressor = 0;
rwcjoliver 0:4788e1df7b55 390
rwcjoliver 0:4788e1df7b55 391 // TOGGLE MOTOR CONTROLLER DEADMAN (SEAT SWITCH AND FOOTBRAKE)
rwcjoliver 0:4788e1df7b55 392 if (rtc.deadman == 0) { // IF DEADMAN PRESSED
rwcjoliver 0:4788e1df7b55 393 motor1.closeDeadman();
rwcjoliver 0:4788e1df7b55 394 }
rwcjoliver 0:4788e1df7b55 395 else {
rwcjoliver 0:4788e1df7b55 396 motor1.releaseDeadman();
rwcjoliver 0:4788e1df7b55 397 }
rwcjoliver 0:4788e1df7b55 398
rwcjoliver 0:4788e1df7b55 399 // TOGGLE REGEN THROTTLING
rwcjoliver 0:4788e1df7b55 400 if (challenge.regenThrottleActive == false) {
rwcjoliver 0:4788e1df7b55 401 if (remote.regenThrottle == 0 && remote.start == 0) { // TURN OFF IF ON
rwcjoliver 0:4788e1df7b55 402 challenge.regenThrottleOn();
rwcjoliver 0:4788e1df7b55 403 }
rwcjoliver 0:4788e1df7b55 404 }
rwcjoliver 0:4788e1df7b55 405 else {
rwcjoliver 0:4788e1df7b55 406 remote.sendError('B'); // Send error to remote
rwcjoliver 0:4788e1df7b55 407 if (remote.regenThrottle == 1) { // TURN ON IF OFF
rwcjoliver 0:4788e1df7b55 408 challenge.regenThrottleOff();
rwcjoliver 0:4788e1df7b55 409 }
rwcjoliver 0:4788e1df7b55 410 }
rwcjoliver 0:4788e1df7b55 411
rwcjoliver 0:4788e1df7b55 412 // TOGGLE REGEN BRAKING
rwcjoliver 0:4788e1df7b55 413 if (challenge.regenBrakingActive == false) {
rwcjoliver 0:4788e1df7b55 414 if (remote.regenBrake == 0 && remote.start == 0) { // TURN OFF IF ON
rwcjoliver 0:4788e1df7b55 415 if (challenge.regenBrakingOn() == 0) {
rwcjoliver 0:4788e1df7b55 416 remote.sendError('I'); // Send error to remote
rwcjoliver 0:4788e1df7b55 417 pc.printf("Regen Braking Off - SuperCaps are full\r\n");
rwcjoliver 0:4788e1df7b55 418 }
rwcjoliver 0:4788e1df7b55 419 }
rwcjoliver 0:4788e1df7b55 420 }
rwcjoliver 0:4788e1df7b55 421 else {
rwcjoliver 0:4788e1df7b55 422 remote.sendError('C'); // Send error to remote
rwcjoliver 0:4788e1df7b55 423 if (remote.regenBrake == 1) { // TURN OFF
rwcjoliver 0:4788e1df7b55 424 challenge.regenBrakingOff();
rwcjoliver 0:4788e1df7b55 425 if (superCapVoltage == 1) {
rwcjoliver 0:4788e1df7b55 426 pc.printf("Regen Braking Off - SuperCaps are full\r\n");
rwcjoliver 0:4788e1df7b55 427 remote.sendError('I'); // Send error to remote
rwcjoliver 0:4788e1df7b55 428 }
rwcjoliver 0:4788e1df7b55 429 }
rwcjoliver 0:4788e1df7b55 430 }
rwcjoliver 0:4788e1df7b55 431
rwcjoliver 0:4788e1df7b55 432 // TOGGLE AUTOSTOP
rwcjoliver 0:4788e1df7b55 433 if (challenge.autoStopActive == 0) {
rwcjoliver 0:4788e1df7b55 434 if (remote.autoStop == 0 && remote.start == 0) { // TURN OFF IF ON
rwcjoliver 0:4788e1df7b55 435 challenge.autoStopOn();
rwcjoliver 0:4788e1df7b55 436 }
rwcjoliver 0:4788e1df7b55 437 }
rwcjoliver 0:4788e1df7b55 438 else {
rwcjoliver 0:4788e1df7b55 439 remote.sendError('D'); // Send error to remote
rwcjoliver 0:4788e1df7b55 440 if (remote.autoStop == 1) { // TURN ON IF OFF
rwcjoliver 0:4788e1df7b55 441 challenge.autoStopOff();
rwcjoliver 0:4788e1df7b55 442 }
rwcjoliver 0:4788e1df7b55 443 }
rwcjoliver 0:4788e1df7b55 444
rwcjoliver 0:4788e1df7b55 445 // TOGGLE INNOVATION
rwcjoliver 0:4788e1df7b55 446 if (challenge.innovationActive == 0) {
rwcjoliver 0:4788e1df7b55 447 if (remote.innovation == 0 && remote.start == 0) { // TURN OFF IF ON
rwcjoliver 0:4788e1df7b55 448 if (driveMode == 0) {
rwcjoliver 0:4788e1df7b55 449 challenge.innovationOn();
rwcjoliver 0:4788e1df7b55 450 }
rwcjoliver 0:4788e1df7b55 451 else {
rwcjoliver 0:4788e1df7b55 452 remote.sendError('J'); // Send error to remote
rwcjoliver 0:4788e1df7b55 453 pc.printf("Can only active innovation mode in forward direction\r\n");
rwcjoliver 0:4788e1df7b55 454 }
rwcjoliver 0:4788e1df7b55 455 }
rwcjoliver 0:4788e1df7b55 456 }
rwcjoliver 0:4788e1df7b55 457 else {
rwcjoliver 0:4788e1df7b55 458 remote.sendError('E'); // Send error to remote
rwcjoliver 0:4788e1df7b55 459
rwcjoliver 0:4788e1df7b55 460 if (remote.innovation == 1) { // TURN ON IF OFF
rwcjoliver 0:4788e1df7b55 461 challenge.innovationOff();
rwcjoliver 0:4788e1df7b55 462 }
rwcjoliver 0:4788e1df7b55 463 }
rwcjoliver 0:4788e1df7b55 464
rwcjoliver 0:4788e1df7b55 465
rwcjoliver 0:4788e1df7b55 466 // CONTROL
rwcjoliver 0:4788e1df7b55 467
rwcjoliver 0:4788e1df7b55 468 if (systemOn == false) {
rwcjoliver 0:4788e1df7b55 469
rwcjoliver 0:4788e1df7b55 470 if (remote.start == 1) {
rwcjoliver 0:4788e1df7b55 471
rwcjoliver 0:4788e1df7b55 472 if (millis() - lastErrorMillis > 500) {
rwcjoliver 0:4788e1df7b55 473 remote.sendError('A'); // SEND ERROR MESSAGE 'A' TO REMOTE
rwcjoliver 0:4788e1df7b55 474 lastErrorMillis = millis();
rwcjoliver 0:4788e1df7b55 475 }
rwcjoliver 0:4788e1df7b55 476
rwcjoliver 0:4788e1df7b55 477 motor1.turnOff();
rwcjoliver 0:4788e1df7b55 478 // motor2.turnOff();
rwcjoliver 0:4788e1df7b55 479 }
rwcjoliver 0:4788e1df7b55 480 else {
rwcjoliver 0:4788e1df7b55 481 systemOn = true;
rwcjoliver 0:4788e1df7b55 482 pc.printf("Start Switch is On\r\n");
rwcjoliver 0:4788e1df7b55 483 // startupHealthCheck(); // Run System Startup Health Check - Will stay in here until it passes
rwcjoliver 0:4788e1df7b55 484
rwcjoliver 0:4788e1df7b55 485 motor1.turnOn(); // Turn on motors
rwcjoliver 0:4788e1df7b55 486 // motor2.turnOn();
rwcjoliver 0:4788e1df7b55 487
rwcjoliver 0:4788e1df7b55 488 }
rwcjoliver 0:4788e1df7b55 489 } // END IF SYSTEMON = FALSE
rwcjoliver 0:4788e1df7b55 490 else { // IF SYSTEMON == TRUE
rwcjoliver 0:4788e1df7b55 491 if (remote.start == 1) {
rwcjoliver 0:4788e1df7b55 492 systemOn = false; // WILL STOP ABOVE HERE NEXT LOOP
rwcjoliver 0:4788e1df7b55 493 pc.printf("Start Switch is Off\r\n");
rwcjoliver 0:4788e1df7b55 494 }
rwcjoliver 0:4788e1df7b55 495
rwcjoliver 0:4788e1df7b55 496 if (driveMode != 0 && remote.forward == 0) {
rwcjoliver 0:4788e1df7b55 497 driveMode = 0;
rwcjoliver 0:4788e1df7b55 498 motor1.setForward();
rwcjoliver 0:4788e1df7b55 499 // motor2.setForward();
rwcjoliver 0:4788e1df7b55 500 }
rwcjoliver 0:4788e1df7b55 501 if (driveMode != 1 && remote.reverse == 0) {
rwcjoliver 0:4788e1df7b55 502 driveMode = 1;
rwcjoliver 0:4788e1df7b55 503 motor1.setReverse();
rwcjoliver 0:4788e1df7b55 504 // motor2.setReverse();
rwcjoliver 0:4788e1df7b55 505 }
rwcjoliver 0:4788e1df7b55 506 if (driveMode != 2 && remote.park == 0) {
rwcjoliver 0:4788e1df7b55 507 driveMode = 2;
rwcjoliver 0:4788e1df7b55 508 motor1.setPark();
rwcjoliver 0:4788e1df7b55 509 motor1.throttle(0);
rwcjoliver 0:4788e1df7b55 510 // motor2.setPark();
rwcjoliver 0:4788e1df7b55 511 }
rwcjoliver 0:4788e1df7b55 512
rwcjoliver 0:4788e1df7b55 513 if (driveMode == 2) { //place in park mode if selected by driver
rwcjoliver 0:4788e1df7b55 514
rwcjoliver 0:4788e1df7b55 515 // pc.printf("RTC Output is %d\r\n", rtc_output.read());
rwcjoliver 0:4788e1df7b55 516 // pc.printf("EM State Output is %d\r\n", emergencyStopActive);
rwcjoliver 0:4788e1df7b55 517 // pc.printf("ParkMode = %d", inParkMode);
rwcjoliver 0:4788e1df7b55 518
rwcjoliver 0:4788e1df7b55 519 if (inParkMode == false) {
rwcjoliver 0:4788e1df7b55 520 pc.printf("Train in park mode.\r\n");
rwcjoliver 0:4788e1df7b55 521 }
rwcjoliver 0:4788e1df7b55 522
rwcjoliver 0:4788e1df7b55 523 if (emergencyStopActive == true && rtc_output.read() == 0) { // Clear emergency stop flag
rwcjoliver 0:4788e1df7b55 524 emergencyStopActive = false;
rwcjoliver 0:4788e1df7b55 525 }
rwcjoliver 0:4788e1df7b55 526
rwcjoliver 0:4788e1df7b55 527 led_parkMode = 1;
rwcjoliver 0:4788e1df7b55 528 inParkMode = true; // Stop above debug print from displaying more than once
rwcjoliver 0:4788e1df7b55 529
rwcjoliver 0:4788e1df7b55 530 motor1.setPark();
rwcjoliver 0:4788e1df7b55 531 // motor2.setPark();
rwcjoliver 0:4788e1df7b55 532
rwcjoliver 0:4788e1df7b55 533 }
rwcjoliver 0:4788e1df7b55 534 else{ //else i.e if selected drive mode is forward or reverse
rwcjoliver 0:4788e1df7b55 535 // pc.printf("RTC Output is %d\r\n", rtc_output.read());
rwcjoliver 0:4788e1df7b55 536 // pc.printf("EM State Output is %d\r\n", emergencyStopActive);
rwcjoliver 0:4788e1df7b55 537 if (emergencyStopActive == false && rtc_output.read() == 0) { // IF RTC FLAGGED AS SAFE
rwcjoliver 0:4788e1df7b55 538
rwcjoliver 0:4788e1df7b55 539 // if (dashboard.currentSpeed < 1 || driveMode == lastKnownDirection) { //do not allow motors to reverse if significant forward speed exists
rwcjoliver 0:4788e1df7b55 540
rwcjoliver 0:4788e1df7b55 541 led_parkMode = 0;
rwcjoliver 0:4788e1df7b55 542 inParkMode = false;
rwcjoliver 0:4788e1df7b55 543
rwcjoliver 0:4788e1df7b55 544 if (driveMode != lastKnownDirection) {
rwcjoliver 0:4788e1df7b55 545 pc.printf("Train enabled for direction %d\r\n", driveMode);
rwcjoliver 0:4788e1df7b55 546
rwcjoliver 0:4788e1df7b55 547 lastKnownDirection = driveMode;
rwcjoliver 0:4788e1df7b55 548 }
rwcjoliver 0:4788e1df7b55 549
rwcjoliver 0:4788e1df7b55 550
rwcjoliver 0:4788e1df7b55 551 if (challenge.autoStopInProgress == true) { // IF AUTOSTOPPING, PASS THROTTLE CONTROL TO FUNCTION
rwcjoliver 0:4788e1df7b55 552 challenge.autoStopControl();
rwcjoliver 0:4788e1df7b55 553 pc.printf("Autostop in Control\r\n");
rwcjoliver 0:4788e1df7b55 554 }
rwcjoliver 0:4788e1df7b55 555 else { // OTHERWISE INPUT THROTTLE FROM REMOTE
rwcjoliver 0:4788e1df7b55 556 if (remote.throttle > 0) { // If joystick pushed upwards = throttle
rwcjoliver 0:4788e1df7b55 557
rwcjoliver 0:4788e1df7b55 558 if (challenge.innovationActive == true) {
rwcjoliver 0:4788e1df7b55 559 pc.printf("Collision Detection in Control\r\n");
rwcjoliver 0:4788e1df7b55 560 int innovationThrottle = challenge.innovationControl(remote.throttle);
rwcjoliver 0:4788e1df7b55 561 speedControl(innovationThrottle);
rwcjoliver 0:4788e1df7b55 562
rwcjoliver 0:4788e1df7b55 563 if (innovationThrottle == 0) {
rwcjoliver 0:4788e1df7b55 564 emergencyStop(); // emergency Brake if obstacle detected
rwcjoliver 0:4788e1df7b55 565 }
rwcjoliver 0:4788e1df7b55 566 }
rwcjoliver 0:4788e1df7b55 567 else {
rwcjoliver 0:4788e1df7b55 568 speedControl(remote.throttle);
rwcjoliver 0:4788e1df7b55 569 pc.printf("Throttling: %d\r\n", remote.throttle);
rwcjoliver 0:4788e1df7b55 570 }
rwcjoliver 0:4788e1df7b55 571 } // remote.throttle
rwcjoliver 0:4788e1df7b55 572 else {
rwcjoliver 0:4788e1df7b55 573 speedControl(0);
rwcjoliver 0:4788e1df7b55 574 }
rwcjoliver 0:4788e1df7b55 575 }
rwcjoliver 0:4788e1df7b55 576
rwcjoliver 0:4788e1df7b55 577 // }
rwcjoliver 0:4788e1df7b55 578 // else {
rwcjoliver 0:4788e1df7b55 579 // pc.printf("Cannot change direction until train has stopped moving\r\n");
rwcjoliver 0:4788e1df7b55 580 // remote.sendError('F'); // Send error to remote
rwcjoliver 0:4788e1df7b55 581 // }
rwcjoliver 0:4788e1df7b55 582 }
rwcjoliver 0:4788e1df7b55 583 else {
rwcjoliver 0:4788e1df7b55 584 pc.printf("Cannot exit park mode until RTC is cleared\r\n");
rwcjoliver 0:4788e1df7b55 585 inParkMode = false;
rwcjoliver 0:4788e1df7b55 586 remote.sendError('G'); // Send error to remote
rwcjoliver 0:4788e1df7b55 587 }
rwcjoliver 0:4788e1df7b55 588 }
rwcjoliver 0:4788e1df7b55 589 } // END IF (SYSTEMON == TRUE)
rwcjoliver 0:4788e1df7b55 590
rwcjoliver 0:4788e1df7b55 591 wait_ms(500); // SLOW DOWN THE SYSTEM (REMOTE CANT KEEP UP)
rwcjoliver 0:4788e1df7b55 592 } // END WHILE(COMMSGOOD)
rwcjoliver 0:4788e1df7b55 593 pc.printf("Main Loop Skipped Due To Emergency Status\r\n");
rwcjoliver 0:4788e1df7b55 594 emergencyStop(); // Emergency stop if comms lost with remote controller
rwcjoliver 0:4788e1df7b55 595
rwcjoliver 0:4788e1df7b55 596 } //END WHILE(1)
rwcjoliver 0:4788e1df7b55 597 }