Railway Challenge / Mbed 2 deprecated challenge

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

Dependencies:   mbed millis

main.cpp@16:7c8ef0e0beaa, 2022-05-04 (annotated)

Committer:
cdevarakonda
Date:
Wed May 04 13:36:30 2022 +0000
Revision:
16:7c8ef0e0beaa
Parent:
15:4976d145fbd9
Child:
17:a5d9c9a45cbc
Updated toggle on park mode and changes inParkMode flag and motor1.setPark() function invoke before parkmode check in main loop. Changed comments highlight the thought process

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;
jamesmcildowietfl 15:4976d145fbd9 32 displayline << "Blackbox# " << " Motor Accelerator: " << motorAccelerator << " Brake 3/2: " << brakeValve32 << " Brake 2/2: " << brakeValve22 << " Speed: " << dashboard.currentSpeed << " Distance: " << dashboard.currentDistance << " Drive Mode: " << driveMode;
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) {
jamesmcildowietfl 8:0fe9f7bde2f9 62 if (driveMode == 2) { // PARK MODE
jamesmcildowietfl 8:0fe9f7bde2f9 63 // All Mechanical brakes applied
jamesmcildowietfl 8:0fe9f7bde2f9 64 motor1.throttle(0.0f);
jamesmcildowietfl 8:0fe9f7bde2f9 65 brakeValve32 = 0;
jamesmcildowietfl 8:0fe9f7bde2f9 66 brakeValve22 = 0;
cdevarakonda 16:7c8ef0e0beaa 67 inParkMode = true; //This toggle was missing, could be the issue
jamesmcildowietfl 8:0fe9f7bde2f9 68 }
jamesmcildowietfl 8:0fe9f7bde2f9 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 }
jamesmcildowietfl 8:0fe9f7bde2f9 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 16:7c8ef0e0beaa 326 motor1.setPark(); //function set to here instead of after print.
jamesmcildowietfl 15:4976d145fbd9 327
cdevarakonda 16:7c8ef0e0beaa 328 if (inParkMode == true) { //changes from false to true
jamesmcildowietfl 8:0fe9f7bde2f9 329 pc.printf("Train in park mode.\r\n"); //why?
jamesmcildowietfl 8:0fe9f7bde2f9 330 }
jamesmcildowietfl 8:0fe9f7bde2f9 331
jamesmcildowietfl 8:0fe9f7bde2f9 332 if (emergencyStopActive == true && rtc_output.read() == 0) { // Clear emergency stop flag
jamesmcildowietfl 8:0fe9f7bde2f9 333 emergencyStopActive = false;
jamesmcildowietfl 8:0fe9f7bde2f9 334 }
jamesmcildowietfl 8:0fe9f7bde2f9 335
jamesmcildowietfl 8:0fe9f7bde2f9 336 led_parkMode = 1;
cdevarakonda 16:7c8ef0e0beaa 337 // inParkMode = true; // Stop above debug print from displaying more than once // commented out as unsure if needed
cdevarakonda 16:7c8ef0e0beaa 338
jamesmcildowietfl 8:0fe9f7bde2f9 339 }
jamesmcildowietfl 8:0fe9f7bde2f9 340
jamesmcildowietfl 8:0fe9f7bde2f9 341 ////Drive
jamesmcildowietfl 8:0fe9f7bde2f9 342 else { //else i.e if selected drive mode is forward or reverse
jamesmcildowietfl 8:0fe9f7bde2f9 343 ////////////////// Start of check for error G RTC clear
jamesmcildowietfl 8:0fe9f7bde2f9 344 if (emergencyStopActive == false && rtc_output.read() == 0) { // IF RTC FLAGGED AS SAFE
jamesmcildowietfl 8:0fe9f7bde2f9 345
jamesmcildowietfl 8:0fe9f7bde2f9 346 led_parkMode = 0;
jamesmcildowietfl 8:0fe9f7bde2f9 347 inParkMode = false;
jamesmcildowietfl 8:0fe9f7bde2f9 348
jamesmcildowietfl 8:0fe9f7bde2f9 349 if (driveMode != lastKnownDirection) {
jamesmcildowietfl 8:0fe9f7bde2f9 350 pc.printf("Train enabled for direction %d\r\n", driveMode);
jamesmcildowietfl 8:0fe9f7bde2f9 351
jamesmcildowietfl 8:0fe9f7bde2f9 352 lastKnownDirection = driveMode;
rwcjoliver 0:4788e1df7b55 353 }
rwcjoliver 0:4788e1df7b55 354
jamesmcildowietfl 8:0fe9f7bde2f9 355 ////Call autostop challenge
jamesmcildowietfl 8:0fe9f7bde2f9 356 if (challenge.autoStopInProgress == true) { // IF AUTOSTOPPING, PASS THROTTLE CONTROL TO FUNCTION
jamesmcildowietfl 8:0fe9f7bde2f9 357 challenge.autoStopControl();
jamesmcildowietfl 8:0fe9f7bde2f9 358 pc.printf("Autostop in Control\r\n");
rwcjoliver 0:4788e1df7b55 359 }
jamesmcildowietfl 8:0fe9f7bde2f9 360 //Use controls from remote
jamesmcildowietfl 8:0fe9f7bde2f9 361 else { // OTHERWISE INPUT THROTTLE FROM REMOTE
jamesmcildowietfl 8:0fe9f7bde2f9 362 if (remote.throttle > 0) { // If joystick pushed upwards = throttle
jamesmcildowietfl 15:4976d145fbd9 363
jamesmcildowietfl 15:4976d145fbd9 364
jamesmcildowietfl 8:0fe9f7bde2f9 365 /////////////////////////Innovation braking start
jamesmcildowietfl 8:0fe9f7bde2f9 366 if (challenge.innovationActive == true) {
jamesmcildowietfl 8:0fe9f7bde2f9 367 pc.printf("Collision Detection in Control\r\n");
jamesmcildowietfl 8:0fe9f7bde2f9 368 int innovationThrottle = challenge.innovationControl(remote.throttle);
jamesmcildowietfl 8:0fe9f7bde2f9 369 speedControl(innovationThrottle);
jamesmcildowietfl 8:0fe9f7bde2f9 370
jamesmcildowietfl 8:0fe9f7bde2f9 371 if (innovationThrottle == 0) {
jamesmcildowietfl 8:0fe9f7bde2f9 372 emergencyStop(); // emergency Brake if obstacle detected
jamesmcildowietfl 8:0fe9f7bde2f9 373 }
rwcjoliver 0:4788e1df7b55 374 }
jamesmcildowietfl 8:0fe9f7bde2f9 375 /////////////////////////Innovation braking end
jamesmcildowietfl 8:0fe9f7bde2f9 376 //normal throttle control
jamesmcildowietfl 8:0fe9f7bde2f9 377 else {
jamesmcildowietfl 8:0fe9f7bde2f9 378 speedControl(remote.throttle);
jamesmcildowietfl 8:0fe9f7bde2f9 379 pc.printf("Throttling: %d\r\n", remote.throttle);
jamesmcildowietfl 8:0fe9f7bde2f9 380 }
jamesmcildowietfl 8:0fe9f7bde2f9 381 } // remote.throttle
jamesmcildowietfl 8:0fe9f7bde2f9 382 ///if nothing set to 0
jamesmcildowietfl 8:0fe9f7bde2f9 383 else {
jamesmcildowietfl 15:4976d145fbd9 384 ////Default Brakes on when speed is 0
jamesmcildowietfl 15:4976d145fbd9 385 if (dashboard.currentSpeed == 0 && remote.throttle == 0) {
jamesmcildowietfl 15:4976d145fbd9 386 brakeControl(4);
jamesmcildowietfl 15:4976d145fbd9 387 }
jamesmcildowietfl 15:4976d145fbd9 388 /////////////////////Left it commented out for now as unsure of the motor throttle check or to use the set speed variable
jamesmcildowietfl 15:4976d145fbd9 389 else {
jamesmcildowietfl 15:4976d145fbd9 390 speedControl(0);
jamesmcildowietfl 15:4976d145fbd9 391 }
jamesmcildowietfl 8:0fe9f7bde2f9 392 }
rwcjoliver 0:4788e1df7b55 393 }
rwcjoliver 0:4788e1df7b55 394
jamesmcildowietfl 8:0fe9f7bde2f9 395 }
jamesmcildowietfl 8:0fe9f7bde2f9 396 ////////////////// End of check for error G RTC clear
jamesmcildowietfl 8:0fe9f7bde2f9 397 else {
jamesmcildowietfl 8:0fe9f7bde2f9 398 pc.printf("Cannot exit park mode until RTC is cleared\r\n");
jamesmcildowietfl 8:0fe9f7bde2f9 399 inParkMode = false;
jamesmcildowietfl 8:0fe9f7bde2f9 400 remote.sendError('G'); // Send error to remote
jamesmcildowietfl 8:0fe9f7bde2f9 401 }
jamesmcildowietfl 8:0fe9f7bde2f9 402 }
jamesmcildowietfl 8:0fe9f7bde2f9 403 //Datalogger Chai Funciton
jamesmcildowietfl 8:0fe9f7bde2f9 404 DisplaySerial();
jamesmcildowietfl 8:0fe9f7bde2f9 405 } // END IF (SYSTEMON == TRUE)
jamesmcildowietfl 8:0fe9f7bde2f9 406 //If train is switched on and in start do this end///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
rwcjoliver 0:4788e1df7b55 407
jamesmcildowietfl 8:0fe9f7bde2f9 408 wait_ms(500); // SLOW DOWN THE SYSTEM (REMOTE CANT KEEP UP)
jamesmcildowietfl 8:0fe9f7bde2f9 409 } // END WHILE(COMMSGOOD)
jamesmcildowietfl 8:0fe9f7bde2f9 410 pc.printf("Main Loop Skipped Due To Emergency Status\r\n");
jamesmcildowietfl 8:0fe9f7bde2f9 411 emergencyStop(); // Emergency stop if comms lost with remote controller
rwcjoliver 0:4788e1df7b55 412
rwcjoliver 0:4788e1df7b55 413
jamesmcildowietfl 8:0fe9f7bde2f9 414 } //END WHILE(1)
jamesmcildowietfl 8:0fe9f7bde2f9 415 }