Railway Challenge / Mbed 2 deprecated challenge

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

Dependencies:   mbed millis

main.cpp@28:ae88b18b5cb7, 2022-06-15 (annotated)

Committer:
cdevarakonda
Date:
Wed Jun 15 11:13:32 2022 +0000
Revision:
28:ae88b18b5cb7
Parent:
26:37450381ffe9
Child:
29:8149eec6d6e7
changed pin outs for brakes through entire code to reflect new assignment, added back park mode redundancy check

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 18:d28d458824d4 30 void DisplaySerial(){
jamesmcildowietfl 18:d28d458824d4 31 std::stringstream displayline;
jamesmcildowietfl 19:a4afd3a6bdfb 32 displayline << "Blackbox# " << " Motor Accelerator: " << motorAccelerator << " Brake 3/2: " << brakeValve32 << " Brake 2/2: " << brakeValve22 << " Speed: " << dashboard.currentSpeed << " Distance: " << dashboard.currentDistance << " Drive Mode: " << driveMode << "\n";
jamesmcildowietfl 20:0add93a69df5 33 string disp = displayline.str();
jamesmcildowietfl 20:0add93a69df5 34 pc.printf("%s \n", disp.c_str());
jamesmcildowietfl 18:d28d458824d4 35 }
jamesmcildowietfl 18:d28d458824d4 36
jamesmcildowietfl 18:d28d458824d4 37 //Emergency Stop
jamesmcildowietfl 8:0fe9f7bde2f9 38 void emergencyStop() { //Emergency Stop Function
jamesmcildowietfl 8:0fe9f7bde2f9 39 // pc.printf("Emergency Stop Active\r\n");
jamesmcildowietfl 8:0fe9f7bde2f9 40 if (emergencyStopActive == false) {
jamesmcildowietfl 3:32e951e05a5b 41
jamesmcildowietfl 8:0fe9f7bde2f9 42 emergencyStopActive = true;
jamesmcildowietfl 18:d28d458824d4 43
jamesmcildowietfl 8:0fe9f7bde2f9 44 //Set brake throttle to zero before disconnected, think is why we had the runaway train imo
jamesmcildowietfl 8:0fe9f7bde2f9 45 motor1.throttle(0);
rwcjoliver 0:4788e1df7b55 46
jamesmcildowietfl 8:0fe9f7bde2f9 47 //Disengage motor
jamesmcildowietfl 18:d28d458824d4 48 motor1.disengage();
jamesmcildowietfl 8:0fe9f7bde2f9 49
jamesmcildowietfl 8:0fe9f7bde2f9 50 //Setting brakes to high
cdevarakonda 28:ae88b18b5cb7 51 brakeValve32 = 0;//(PD_3)
cdevarakonda 28:ae88b18b5cb7 52 brakeValve22 = 0;//(PC_0)
jamesmcildowietfl 8:0fe9f7bde2f9 53 if (rtc_output.read() == 1) { //Check RTC pin out
jamesmcildowietfl 8:0fe9f7bde2f9 54 rtc.getTriggerCause(); // Get RTC input status
rwcjoliver 0:4788e1df7b55 55 }
jamesmcildowietfl 8:0fe9f7bde2f9 56 }
rwcjoliver 0:4788e1df7b55 57 }
rwcjoliver 0:4788e1df7b55 58
jamesmcildowietfl 18:d28d458824d4 59 //Brake code
rwcjoliver 0:4788e1df7b55 60 void brakeControl(int brakeRate) {
jamesmcildowietfl 18:d28d458824d4 61 if (driveMode == 2) { // PARK MODE
jamesmcildowietfl 8:0fe9f7bde2f9 62 // All Mechanical brakes applied
jamesmcildowietfl 18:d28d458824d4 63 motor1.throttle(0.0f);
jamesmcildowietfl 18:d28d458824d4 64 brakeValve32 = 0;
jamesmcildowietfl 18:d28d458824d4 65 brakeValve22 = 0;
cdevarakonda 28:ae88b18b5cb7 66 inParkMode=true;
jamesmcildowietfl 18:d28d458824d4 67 else {//REGEN BRAKING
jamesmcildowietfl 8:0fe9f7bde2f9 68 if (challenge.regenBrakingActive == true) { // REGEN BRAKING WITH OVERVOLTAGE SAFETY CHECK
jamesmcildowietfl 8:0fe9f7bde2f9 69 if (brakeRate > 0) {
cdevarakonda 26:37450381ffe9 70 motor1.setPark();
cdevarakonda 26:37450381ffe9 71 //motor1.brake(brakeRate);
jamesmcildowietfl 8:0fe9f7bde2f9 72 }
jamesmcildowietfl 8:0fe9f7bde2f9 73 else {
jamesmcildowietfl 8:0fe9f7bde2f9 74 motor1.setForward();
jamesmcildowietfl 8:0fe9f7bde2f9 75 }
rwcjoliver 0:4788e1df7b55 76 }
jamesmcildowietfl 8:0fe9f7bde2f9 77 else { // MECHANICAL BRAKING
jamesmcildowietfl 8:0fe9f7bde2f9 78 switch (brakeRate) {
jamesmcildowietfl 8:0fe9f7bde2f9 79 case 0: // NO BRAKING
cdevarakonda 28:ae88b18b5cb7 80 brakeValve32 = 1;//(PD_3)
cdevarakonda 28:ae88b18b5cb7 81 brakeValve22 = 1;//(PC_0)
jamesmcildowietfl 8:0fe9f7bde2f9 82 break;
jamesmcildowietfl 8:0fe9f7bde2f9 83
jamesmcildowietfl 8:0fe9f7bde2f9 84 case 1: //HALF BRAKING
jamesmcildowietfl 8:0fe9f7bde2f9 85 motor1.throttle(0.0f);
cdevarakonda 28:ae88b18b5cb7 86 brakeValve32 = 0;//(PD_3)
cdevarakonda 28:ae88b18b5cb7 87 brakeValve22 = 1;//(PC_0)
jamesmcildowietfl 8:0fe9f7bde2f9 88 break;
jamesmcildowietfl 2:d8d92dfc9c82 89
jamesmcildowietfl 8:0fe9f7bde2f9 90 case 2 ... 4 : //FULL BRAKING
jamesmcildowietfl 8:0fe9f7bde2f9 91 motor1.throttle(0.0f);
cdevarakonda 28:ae88b18b5cb7 92 brakeValve32 = 0;//(PD_3)
cdevarakonda 28:ae88b18b5cb7 93 brakeValve22 = 0;//(PC_0)
jamesmcildowietfl 8:0fe9f7bde2f9 94 break;
jamesmcildowietfl 8:0fe9f7bde2f9 95
jamesmcildowietfl 8:0fe9f7bde2f9 96 default: // NO BRAKING
cdevarakonda 28:ae88b18b5cb7 97 brakeValve32 = 1;//(PD_3)
cdevarakonda 28:ae88b18b5cb7 98 brakeValve22 = 1;//(PC_0)
jamesmcildowietfl 8:0fe9f7bde2f9 99 break;
jamesmcildowietfl 8:0fe9f7bde2f9 100 }
jamesmcildowietfl 8:0fe9f7bde2f9 101 }
jamesmcildowietfl 18:d28d458824d4 102 }
jamesmcildowietfl 8:0fe9f7bde2f9 103 return;
rwcjoliver 0:4788e1df7b55 104 }
rwcjoliver 0:4788e1df7b55 105
jamesmcildowietfl 8:0fe9f7bde2f9 106 //Motor code
rwcjoliver 0:4788e1df7b55 107 void speedControl(int commandedSpeed) {
jamesmcildowietfl 8:0fe9f7bde2f9 108 switch (commandedSpeed) {
jamesmcildowietfl 8:0fe9f7bde2f9 109
jamesmcildowietfl 8:0fe9f7bde2f9 110 default:
jamesmcildowietfl 8:0fe9f7bde2f9 111 break;
jamesmcildowietfl 8:0fe9f7bde2f9 112
jamesmcildowietfl 8:0fe9f7bde2f9 113 case 0:
jamesmcildowietfl 8:0fe9f7bde2f9 114 motor1.throttle(0.0f);
jamesmcildowietfl 8:0fe9f7bde2f9 115 break;
jamesmcildowietfl 8:0fe9f7bde2f9 116
jamesmcildowietfl 8:0fe9f7bde2f9 117 case 1 ... 2:
jamesmcildowietfl 8:0fe9f7bde2f9 118 motor1.throttle(0.1f);
jamesmcildowietfl 8:0fe9f7bde2f9 119 break;
jamesmcildowietfl 8:0fe9f7bde2f9 120
jamesmcildowietfl 8:0fe9f7bde2f9 121 case 3 ... 4:
jamesmcildowietfl 8:0fe9f7bde2f9 122 motor1.throttle(0.2f);
jamesmcildowietfl 8:0fe9f7bde2f9 123 break;
jamesmcildowietfl 8:0fe9f7bde2f9 124
jamesmcildowietfl 8:0fe9f7bde2f9 125 case 5 ... 6:
jamesmcildowietfl 8:0fe9f7bde2f9 126 motor1.throttle(0.3f);
jamesmcildowietfl 8:0fe9f7bde2f9 127 break;
jamesmcildowietfl 8:0fe9f7bde2f9 128
jamesmcildowietfl 8:0fe9f7bde2f9 129 case 7 ... 8:
jamesmcildowietfl 8:0fe9f7bde2f9 130 motor1.throttle(0.4f);
jamesmcildowietfl 8:0fe9f7bde2f9 131 break;
jamesmcildowietfl 8:0fe9f7bde2f9 132
jamesmcildowietfl 8:0fe9f7bde2f9 133 case 9 ... 10:
jamesmcildowietfl 8:0fe9f7bde2f9 134 motor1.throttle(0.5f);
jamesmcildowietfl 8:0fe9f7bde2f9 135 break;
jamesmcildowietfl 8:0fe9f7bde2f9 136
jamesmcildowietfl 8:0fe9f7bde2f9 137 case 11:
jamesmcildowietfl 8:0fe9f7bde2f9 138 motor1.throttle(0.6f);
jamesmcildowietfl 8:0fe9f7bde2f9 139 break;
jamesmcildowietfl 8:0fe9f7bde2f9 140
jamesmcildowietfl 8:0fe9f7bde2f9 141 case 12:
jamesmcildowietfl 8:0fe9f7bde2f9 142 motor1.throttle(0.7f);
jamesmcildowietfl 8:0fe9f7bde2f9 143 break;
jamesmcildowietfl 8:0fe9f7bde2f9 144
jamesmcildowietfl 8:0fe9f7bde2f9 145 case 13:
jamesmcildowietfl 8:0fe9f7bde2f9 146 motor1.throttle(0.8f);
jamesmcildowietfl 8:0fe9f7bde2f9 147 break;
jamesmcildowietfl 8:0fe9f7bde2f9 148
jamesmcildowietfl 8:0fe9f7bde2f9 149 case 14:
jamesmcildowietfl 8:0fe9f7bde2f9 150 motor1.throttle(0.9f);
jamesmcildowietfl 8:0fe9f7bde2f9 151 break;
jamesmcildowietfl 8:0fe9f7bde2f9 152
jamesmcildowietfl 8:0fe9f7bde2f9 153 case 15:
jamesmcildowietfl 8:0fe9f7bde2f9 154 motor1.throttle(1.0f);
jamesmcildowietfl 8:0fe9f7bde2f9 155 break;
jamesmcildowietfl 8:0fe9f7bde2f9 156 }
rwcjoliver 0:4788e1df7b55 157 }
rwcjoliver 0:4788e1df7b55 158
jamesmcildowietfl 8:0fe9f7bde2f9 159 int main() {
jamesmcildowietfl 8:0fe9f7bde2f9 160 pc.baud(115200);
jamesmcildowietfl 8:0fe9f7bde2f9 161
jamesmcildowietfl 8:0fe9f7bde2f9 162 // CONFIGURE INTERRUPTS
jamesmcildowietfl 8:0fe9f7bde2f9 163 rtc_output.rise(&emergencyStop);
jamesmcildowietfl 8:0fe9f7bde2f9 164
jamesmcildowietfl 8:0fe9f7bde2f9 165 millisStart();
jamesmcildowietfl 8:0fe9f7bde2f9 166
jamesmcildowietfl 8:0fe9f7bde2f9 167 rtc_Trigger = 1;
jamesmcildowietfl 8:0fe9f7bde2f9 168
jamesmcildowietfl 8:0fe9f7bde2f9 169 // LOCAL VARIABLES
jamesmcildowietfl 8:0fe9f7bde2f9 170 bool systemOn = false; // On/Off status of loco
jamesmcildowietfl 8:0fe9f7bde2f9 171 int lastKnownDirection = 2;
jamesmcildowietfl 8:0fe9f7bde2f9 172 bool inParkMode = false;
jamesmcildowietfl 8:0fe9f7bde2f9 173
jamesmcildowietfl 8:0fe9f7bde2f9 174 // Record last time error was sent
jamesmcildowietfl 8:0fe9f7bde2f9 175 unsigned long lastErrorMillis = 0;
jamesmcildowietfl 8:0fe9f7bde2f9 176
jamesmcildowietfl 8:0fe9f7bde2f9 177 //MainLoop
jamesmcildowietfl 8:0fe9f7bde2f9 178 while (1) {
jamesmcildowietfl 8:0fe9f7bde2f9 179
jamesmcildowietfl 8:0fe9f7bde2f9 180 while (remote.commsGood == true) {
jamesmcildowietfl 8:0fe9f7bde2f9 181 /////Start Up///////////////////////////////////////////////////////////////////////////////////////////////////////////
jamesmcildowietfl 8:0fe9f7bde2f9 182
jamesmcildowietfl 8:0fe9f7bde2f9 183 /////Checking Modes from controller/////////////////////////////////////////////////////////////////////////////////
jamesmcildowietfl 8:0fe9f7bde2f9 184 // PING
jamesmcildowietfl 8:0fe9f7bde2f9 185 remote.commsCheck();
jamesmcildowietfl 8:0fe9f7bde2f9 186
jamesmcildowietfl 8:0fe9f7bde2f9 187 // GET SWITCH STATES
jamesmcildowietfl 8:0fe9f7bde2f9 188 remote.getSwitchStates();
jamesmcildowietfl 8:0fe9f7bde2f9 189
jamesmcildowietfl 8:0fe9f7bde2f9 190 // ALLOW BRAKES TO BE OPERATED
jamesmcildowietfl 8:0fe9f7bde2f9 191 brakeControl(remote.braking);
jamesmcildowietfl 8:0fe9f7bde2f9 192
jamesmcildowietfl 8:0fe9f7bde2f9 193 // SOUND WHISTLE IF WHISTLE BUTTON PRESSED
jamesmcildowietfl 8:0fe9f7bde2f9 194 if (remote.whistle == 0) {
jamesmcildowietfl 8:0fe9f7bde2f9 195 whistleValve32 = 1;
jamesmcildowietfl 8:0fe9f7bde2f9 196 }
jamesmcildowietfl 8:0fe9f7bde2f9 197 else {
jamesmcildowietfl 8:0fe9f7bde2f9 198 whistleValve32 = 0;
jamesmcildowietfl 8:0fe9f7bde2f9 199 }
jamesmcildowietfl 8:0fe9f7bde2f9 200
jamesmcildowietfl 8:0fe9f7bde2f9 201 // GET AND DISPLAY SPEED
jamesmcildowietfl 8:0fe9f7bde2f9 202 dashboard.getCurrentSpeed();
jamesmcildowietfl 8:0fe9f7bde2f9 203 remote.sendData(2, dashboard.currentSpeed); // Send speed to remote
jamesmcildowietfl 8:0fe9f7bde2f9 204
jamesmcildowietfl 8:0fe9f7bde2f9 205 // TOGGLE COMPRESSOR
jamesmcildowietfl 8:0fe9f7bde2f9 206 remote.compressor == 0 ? contactCompressor = 1 : contactCompressor = 0;
jamesmcildowietfl 8:0fe9f7bde2f9 207
jamesmcildowietfl 8:0fe9f7bde2f9 208 // TOGGLE MOTOR CONTROLLER DEADMAN (SEAT SWITCH AND FOOTBRAKE)
jamesmcildowietfl 8:0fe9f7bde2f9 209 if (rtc.deadman == 0) { // IF DEADMAN PRESSED
jamesmcildowietfl 8:0fe9f7bde2f9 210 motor1.closeDeadman();
jamesmcildowietfl 8:0fe9f7bde2f9 211 }
jamesmcildowietfl 8:0fe9f7bde2f9 212 else {
jamesmcildowietfl 8:0fe9f7bde2f9 213 motor1.releaseDeadman();
jamesmcildowietfl 8:0fe9f7bde2f9 214 }
jamesmcildowietfl 8:0fe9f7bde2f9 215
jamesmcildowietfl 8:0fe9f7bde2f9 216 // TOGGLE REGEN THROTTLING
jamesmcildowietfl 8:0fe9f7bde2f9 217 if (challenge.regenThrottleActive == false) {
jamesmcildowietfl 8:0fe9f7bde2f9 218 if (remote.regenThrottle == 0 && remote.start == 0) { // TURN OFF IF ON
jamesmcildowietfl 8:0fe9f7bde2f9 219 challenge.regenThrottleOn();
rwcjoliver 0:4788e1df7b55 220 }
jamesmcildowietfl 8:0fe9f7bde2f9 221 }
jamesmcildowietfl 8:0fe9f7bde2f9 222 else {
jamesmcildowietfl 8:0fe9f7bde2f9 223 remote.sendError('B'); // Send error to remote
jamesmcildowietfl 8:0fe9f7bde2f9 224 if (remote.regenThrottle == 1) { // TURN ON IF OFF
jamesmcildowietfl 8:0fe9f7bde2f9 225 challenge.regenThrottleOff();
jamesmcildowietfl 8:0fe9f7bde2f9 226 }
jamesmcildowietfl 8:0fe9f7bde2f9 227 }
rwcjoliver 0:4788e1df7b55 228
jamesmcildowietfl 8:0fe9f7bde2f9 229 // TOGGLE REGEN BRAKING
jamesmcildowietfl 8:0fe9f7bde2f9 230 if (challenge.regenBrakingActive == false) {
jamesmcildowietfl 8:0fe9f7bde2f9 231 if (remote.regenBrake == 0 && remote.start == 0) { // TURN OFF IF ON
jamesmcildowietfl 8:0fe9f7bde2f9 232 if (challenge.regenBrakingOn() == 0) {
jamesmcildowietfl 8:0fe9f7bde2f9 233 remote.sendError('I'); // Send error to remote
jamesmcildowietfl 8:0fe9f7bde2f9 234 pc.printf("Regen Braking Off - SuperCaps are full\r\n");
jamesmcildowietfl 8:0fe9f7bde2f9 235 }
jamesmcildowietfl 8:0fe9f7bde2f9 236 }
jamesmcildowietfl 8:0fe9f7bde2f9 237 }
jamesmcildowietfl 8:0fe9f7bde2f9 238 else {
jamesmcildowietfl 8:0fe9f7bde2f9 239 remote.sendError('C'); // Send error to remote
jamesmcildowietfl 8:0fe9f7bde2f9 240 if (remote.regenBrake == 1) { // TURN OFF
jamesmcildowietfl 8:0fe9f7bde2f9 241 challenge.regenBrakingOff();
jamesmcildowietfl 8:0fe9f7bde2f9 242 if (superCapVoltage == 1) {
jamesmcildowietfl 8:0fe9f7bde2f9 243 pc.printf("Regen Braking Off - SuperCaps are full\r\n");
jamesmcildowietfl 8:0fe9f7bde2f9 244 remote.sendError('I'); // Send error to remote
jamesmcildowietfl 8:0fe9f7bde2f9 245 }
jamesmcildowietfl 8:0fe9f7bde2f9 246 }
jamesmcildowietfl 8:0fe9f7bde2f9 247 }
rwcjoliver 0:4788e1df7b55 248
jamesmcildowietfl 8:0fe9f7bde2f9 249 // TOGGLE AUTOSTOP
jamesmcildowietfl 8:0fe9f7bde2f9 250 if (challenge.autoStopActive == 0) {
jamesmcildowietfl 8:0fe9f7bde2f9 251 if (remote.autoStop == 0 && remote.start == 0) { // TURN OFF IF ON
jamesmcildowietfl 8:0fe9f7bde2f9 252 challenge.autoStopOn();
jamesmcildowietfl 8:0fe9f7bde2f9 253 }
jamesmcildowietfl 8:0fe9f7bde2f9 254 }
jamesmcildowietfl 8:0fe9f7bde2f9 255 else {
jamesmcildowietfl 8:0fe9f7bde2f9 256 remote.sendError('D'); // Send error to remote
jamesmcildowietfl 8:0fe9f7bde2f9 257 if (remote.autoStop == 1) { // TURN ON IF OFF
jamesmcildowietfl 8:0fe9f7bde2f9 258 challenge.autoStopOff();
jamesmcildowietfl 8:0fe9f7bde2f9 259 }
jamesmcildowietfl 8:0fe9f7bde2f9 260 }
rwcjoliver 0:4788e1df7b55 261
jamesmcildowietfl 8:0fe9f7bde2f9 262 // TOGGLE INNOVATION
jamesmcildowietfl 8:0fe9f7bde2f9 263 if (challenge.innovationActive == 0) {
jamesmcildowietfl 8:0fe9f7bde2f9 264 if (remote.innovation == 0 && remote.start == 0) { // TURN OFF IF ON
jamesmcildowietfl 8:0fe9f7bde2f9 265 if (driveMode == 0) {
jamesmcildowietfl 8:0fe9f7bde2f9 266 challenge.innovationOn();
jamesmcildowietfl 8:0fe9f7bde2f9 267 }
jamesmcildowietfl 8:0fe9f7bde2f9 268 else {
jamesmcildowietfl 8:0fe9f7bde2f9 269 remote.sendError('J'); // Send error to remote
jamesmcildowietfl 8:0fe9f7bde2f9 270 pc.printf("Can only active innovation mode in forward direction\r\n");
jamesmcildowietfl 8:0fe9f7bde2f9 271 }
jamesmcildowietfl 8:0fe9f7bde2f9 272 }
jamesmcildowietfl 8:0fe9f7bde2f9 273 }
jamesmcildowietfl 8:0fe9f7bde2f9 274 else {
jamesmcildowietfl 8:0fe9f7bde2f9 275 remote.sendError('E'); // Send error to remote
jamesmcildowietfl 8:0fe9f7bde2f9 276
jamesmcildowietfl 8:0fe9f7bde2f9 277 if (remote.innovation == 1) { // TURN ON IF OFF
jamesmcildowietfl 8:0fe9f7bde2f9 278 challenge.innovationOff();
jamesmcildowietfl 8:0fe9f7bde2f9 279 }
jamesmcildowietfl 8:0fe9f7bde2f9 280 }
jamesmcildowietfl 8:0fe9f7bde2f9 281 /////END OF TOGGLE CHECKS//////////////////////////////////////////////////////////////////////////////////////////////
rwcjoliver 0:4788e1df7b55 282
jamesmcildowietfl 8:0fe9f7bde2f9 283 /////Control///////////////////////////////////////////////////////////////////////////////////////////////////////////
jamesmcildowietfl 8:0fe9f7bde2f9 284 //Is a Toggle check, but it is the train start swtich A
jamesmcildowietfl 8:0fe9f7bde2f9 285 if (systemOn == false) {
jamesmcildowietfl 8:0fe9f7bde2f9 286 if (remote.start == 1) {
jamesmcildowietfl 8:0fe9f7bde2f9 287 if (millis() - lastErrorMillis > 500) {
jamesmcildowietfl 8:0fe9f7bde2f9 288 remote.sendError('A'); // SEND ERROR MESSAGE 'A' TO REMOTE
jamesmcildowietfl 8:0fe9f7bde2f9 289 lastErrorMillis = millis();
jamesmcildowietfl 8:0fe9f7bde2f9 290 }
jamesmcildowietfl 8:0fe9f7bde2f9 291 motor1.turnOff();
jamesmcildowietfl 8:0fe9f7bde2f9 292 }
jamesmcildowietfl 8:0fe9f7bde2f9 293 else {
jamesmcildowietfl 8:0fe9f7bde2f9 294 systemOn = true;
jamesmcildowietfl 8:0fe9f7bde2f9 295 motor1.turnOn(); // Turn on motors
jamesmcildowietfl 8:0fe9f7bde2f9 296 }
jamesmcildowietfl 8:0fe9f7bde2f9 297 } // END IF SYSTEMON = FALSE
jamesmcildowietfl 8:0fe9f7bde2f9 298 //If train is switched on and in start do this start///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jamesmcildowietfl 8:0fe9f7bde2f9 299 else { // IF SYSTEMON == TRUE
jamesmcildowietfl 8:0fe9f7bde2f9 300 if (remote.start == 1) {
jamesmcildowietfl 8:0fe9f7bde2f9 301 systemOn = false; // WILL STOP ABOVE HERE NEXT LOOP
jamesmcildowietfl 8:0fe9f7bde2f9 302 pc.printf("Start Switch is Off\r\n");
jamesmcildowietfl 8:0fe9f7bde2f9 303 }
jamesmcildowietfl 15:4976d145fbd9 304
jamesmcildowietfl 8:0fe9f7bde2f9 305 //Set foward
jamesmcildowietfl 8:0fe9f7bde2f9 306 if (driveMode != 0 && remote.forward == 0) {
jamesmcildowietfl 8:0fe9f7bde2f9 307 driveMode = 0;
jamesmcildowietfl 8:0fe9f7bde2f9 308 motor1.setForward();
jamesmcildowietfl 8:0fe9f7bde2f9 309 }
jamesmcildowietfl 8:0fe9f7bde2f9 310 //Set reverse
jamesmcildowietfl 8:0fe9f7bde2f9 311 if (driveMode != 1 && remote.reverse == 0) {
jamesmcildowietfl 8:0fe9f7bde2f9 312 driveMode = 1;
jamesmcildowietfl 8:0fe9f7bde2f9 313 motor1.setReverse();
jamesmcildowietfl 8:0fe9f7bde2f9 314 }
jamesmcildowietfl 8:0fe9f7bde2f9 315 //Set park
jamesmcildowietfl 8:0fe9f7bde2f9 316 if (driveMode != 2 && remote.park == 0) {
jamesmcildowietfl 8:0fe9f7bde2f9 317 driveMode = 2;
jamesmcildowietfl 8:0fe9f7bde2f9 318 motor1.setPark();
jamesmcildowietfl 8:0fe9f7bde2f9 319 motor1.throttle(0);
jamesmcildowietfl 8:0fe9f7bde2f9 320 }
jamesmcildowietfl 8:0fe9f7bde2f9 321 ////Park Mode
cdevarakonda 28:ae88b18b5cb7 322 if (driveMode == 2) {
cdevarakonda 28:ae88b18b5cb7 323 brakeControl(4); //place in park mode if selected by driver, redundancy check
jamesmcildowietfl 18:d28d458824d4 324 if (inParkMode == false) {
cdevarakonda 28:ae88b18b5cb7 325 //pc.printf("Train in park mode.\r\n"); //why?
jamesmcildowietfl 18:d28d458824d4 326 }
jamesmcildowietfl 8:0fe9f7bde2f9 327
jamesmcildowietfl 8:0fe9f7bde2f9 328 if (emergencyStopActive == true && rtc_output.read() == 0) { // Clear emergency stop flag
jamesmcildowietfl 8:0fe9f7bde2f9 329 emergencyStopActive = false;
jamesmcildowietfl 8:0fe9f7bde2f9 330 }
jamesmcildowietfl 8:0fe9f7bde2f9 331
jamesmcildowietfl 8:0fe9f7bde2f9 332 led_parkMode = 1;
jamesmcildowietfl 18:d28d458824d4 333 inParkMode = true; // Stop above debug print from displaying more than once
jamesmcildowietfl 18:d28d458824d4 334 motor1.setPark();
jamesmcildowietfl 8:0fe9f7bde2f9 335 }
jamesmcildowietfl 8:0fe9f7bde2f9 336
jamesmcildowietfl 8:0fe9f7bde2f9 337 ////Drive
jamesmcildowietfl 8:0fe9f7bde2f9 338 else { //else i.e if selected drive mode is forward or reverse
jamesmcildowietfl 8:0fe9f7bde2f9 339 ////////////////// Start of check for error G RTC clear
jamesmcildowietfl 8:0fe9f7bde2f9 340 if (emergencyStopActive == false && rtc_output.read() == 0) { // IF RTC FLAGGED AS SAFE
jamesmcildowietfl 8:0fe9f7bde2f9 341
jamesmcildowietfl 8:0fe9f7bde2f9 342 led_parkMode = 0;
jamesmcildowietfl 8:0fe9f7bde2f9 343 inParkMode = false;
jamesmcildowietfl 8:0fe9f7bde2f9 344
jamesmcildowietfl 8:0fe9f7bde2f9 345 if (driveMode != lastKnownDirection) {
jamesmcildowietfl 8:0fe9f7bde2f9 346 pc.printf("Train enabled for direction %d\r\n", driveMode);
jamesmcildowietfl 8:0fe9f7bde2f9 347
jamesmcildowietfl 8:0fe9f7bde2f9 348 lastKnownDirection = driveMode;
rwcjoliver 0:4788e1df7b55 349 }
rwcjoliver 0:4788e1df7b55 350
jamesmcildowietfl 8:0fe9f7bde2f9 351 ////Call autostop challenge
jamesmcildowietfl 8:0fe9f7bde2f9 352 if (challenge.autoStopInProgress == true) { // IF AUTOSTOPPING, PASS THROTTLE CONTROL TO FUNCTION
jamesmcildowietfl 8:0fe9f7bde2f9 353 challenge.autoStopControl();
jamesmcildowietfl 8:0fe9f7bde2f9 354 pc.printf("Autostop in Control\r\n");
rwcjoliver 0:4788e1df7b55 355 }
jamesmcildowietfl 8:0fe9f7bde2f9 356 //Use controls from remote
jamesmcildowietfl 8:0fe9f7bde2f9 357 else { // OTHERWISE INPUT THROTTLE FROM REMOTE
jamesmcildowietfl 8:0fe9f7bde2f9 358 if (remote.throttle > 0) { // If joystick pushed upwards = throttle
jamesmcildowietfl 8:0fe9f7bde2f9 359 /////////////////////////Innovation braking start
jamesmcildowietfl 8:0fe9f7bde2f9 360 if (challenge.innovationActive == true) {
jamesmcildowietfl 8:0fe9f7bde2f9 361 pc.printf("Collision Detection in Control\r\n");
jamesmcildowietfl 8:0fe9f7bde2f9 362 int innovationThrottle = challenge.innovationControl(remote.throttle);
jamesmcildowietfl 8:0fe9f7bde2f9 363 speedControl(innovationThrottle);
jamesmcildowietfl 8:0fe9f7bde2f9 364
jamesmcildowietfl 8:0fe9f7bde2f9 365 if (innovationThrottle == 0) {
jamesmcildowietfl 8:0fe9f7bde2f9 366 emergencyStop(); // emergency Brake if obstacle detected
jamesmcildowietfl 8:0fe9f7bde2f9 367 }
rwcjoliver 0:4788e1df7b55 368 }
jamesmcildowietfl 8:0fe9f7bde2f9 369 /////////////////////////Innovation braking end
jamesmcildowietfl 8:0fe9f7bde2f9 370 //normal throttle control
jamesmcildowietfl 8:0fe9f7bde2f9 371 else {
jamesmcildowietfl 8:0fe9f7bde2f9 372 speedControl(remote.throttle);
jamesmcildowietfl 8:0fe9f7bde2f9 373 pc.printf("Throttling: %d\r\n", remote.throttle);
jamesmcildowietfl 8:0fe9f7bde2f9 374 }
jamesmcildowietfl 8:0fe9f7bde2f9 375 } // remote.throttle
jamesmcildowietfl 8:0fe9f7bde2f9 376 ///if nothing set to 0
jamesmcildowietfl 8:0fe9f7bde2f9 377 else {
jamesmcildowietfl 18:d28d458824d4 378 speedControl(0);
jamesmcildowietfl 8:0fe9f7bde2f9 379 }
rwcjoliver 0:4788e1df7b55 380 }
rwcjoliver 0:4788e1df7b55 381
jamesmcildowietfl 8:0fe9f7bde2f9 382 }
jamesmcildowietfl 8:0fe9f7bde2f9 383 ////////////////// End of check for error G RTC clear
jamesmcildowietfl 8:0fe9f7bde2f9 384 else {
jamesmcildowietfl 8:0fe9f7bde2f9 385 pc.printf("Cannot exit park mode until RTC is cleared\r\n");
jamesmcildowietfl 8:0fe9f7bde2f9 386 inParkMode = false;
jamesmcildowietfl 8:0fe9f7bde2f9 387 remote.sendError('G'); // Send error to remote
jamesmcildowietfl 8:0fe9f7bde2f9 388 }
jamesmcildowietfl 8:0fe9f7bde2f9 389 }
jamesmcildowietfl 8:0fe9f7bde2f9 390 //Datalogger Chai Funciton
jamesmcildowietfl 8:0fe9f7bde2f9 391 DisplaySerial();
jamesmcildowietfl 8:0fe9f7bde2f9 392 } // END IF (SYSTEMON == TRUE)
jamesmcildowietfl 8:0fe9f7bde2f9 393 //If train is switched on and in start do this end///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
rwcjoliver 0:4788e1df7b55 394
jamesmcildowietfl 19:a4afd3a6bdfb 395 wait_ms(25); // SLOW DOWN THE SYSTEM (REMOTE CANT KEEP UP)
jamesmcildowietfl 8:0fe9f7bde2f9 396 } // END WHILE(COMMSGOOD)
jamesmcildowietfl 8:0fe9f7bde2f9 397 pc.printf("Main Loop Skipped Due To Emergency Status\r\n");
jamesmcildowietfl 8:0fe9f7bde2f9 398 emergencyStop(); // Emergency stop if comms lost with remote controller
rwcjoliver 0:4788e1df7b55 399
rwcjoliver 0:4788e1df7b55 400
jamesmcildowietfl 8:0fe9f7bde2f9 401 } //END WHILE(1)
jamesmcildowietfl 8:0fe9f7bde2f9 402 }