Railway Challenge / Mbed 2 deprecated challenge

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

Dependencies:   mbed millis

main.cpp@17:a5d9c9a45cbc, 2022-05-04 (annotated)

Committer:
cdevarakonda
Date:
Wed May 04 13:52:22 2022 +0000
Revision:
17:a5d9c9a45cbc
Parent:
16:7c8ef0e0beaa
Child:
18:d28d458824d4
Park Mode condition (if drivemode==2) commented out from brakeControl function as this is being checked again in the main.; ; Instead invoked brakeControl(4) i.e. full braking  under the park mode block in main as this achieves the same result

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