Railway Challenge
Uncommenting of part that allow supercaps to charge up from the batteries
Diff: main.cpp
- Revision:
- 8:0fe9f7bde2f9
- Parent:
- 6:d0ba4c659e9c
- Child:
- 9:d1998035418b
--- a/main.cpp Wed Apr 13 12:44:43 2022 +0000
+++ b/main.cpp Wed Apr 20 15:50:23 2022 +0000
@@ -33,571 +33,368 @@
string disp=displayline.str();
pc.printf("%s \n",disp);
}
+
+//Emergency Stop
+void emergencyStop() { //Emergency Stop Function
+ // pc.printf("Emergency Stop Active\r\n");
+ if (emergencyStopActive == false) {
-void startupHealthCheck() {
-
- while(1) {
+ emergencyStopActive = true;
+
+ //Set brake throttle to zero before disconnected, think is why we had the runaway train imo
+ motor1.throttle(0);
- if (remote.commsGood == 1) {
- if (rtc_output.read() == 0) {
-// if (batteryVoltage == true) {
- if (superCapVoltage == true) {
-//
-//
- return; // Exit the function if all checks are passed
-//
- }
- else {
- pc.printf("System Start-Up Health Check: SuperCap Voltage Check Failed\r\n");
- }
-// }
-// else {
-// pc.printf("System Start-Up Health Check: Battery Voltage Check Failed\r\n");
-// }
- }
- else {
- pc.printf("System Start-Up Health Check: RTC Check Failed\r\n");
- }
- }
- else {
- pc.printf("System Start-Up Health Check: CommsCheck Failed\r\n");
- }
- remote.sendError('H'); // Send error to remote
- wait(100); // Wait a while until trying again
+ //Disengage motor
+ motor1.disengage();
+
+ //Setting brakes to high
+ brakeValve32 = 0;//(PF_2)
+ brakeValve22 = 0;//(PG_1)
+ if (rtc_output.read() == 1) { //Check RTC pin out
+ rtc.getTriggerCause(); // Get RTC input status
}
-}
-
-void emergencyStop() { //Emergency Stop Function
-// pc.printf("Emergency Stop Active\r\n");
- if (emergencyStopActive == false) {
-
- emergencyStopActive = true;
-
- motor1.disengage();
-
- //Setting brakes to high
- brakeValve32 = 0;//(PF_2)
- brakeValve22 = 0;//(PG_1)
- // Disengage both motors
-// motor2.disengage();
-
- // motor1.setPark(); Redundancy as disengage function simply calls SetPark // Clear Motor Directions
-// motor2.setPark();
-
- if (rtc_output.read() == 1) { //Check RTC pin out
- rtc.getTriggerCause(); // Get RTC input status
- }
- }
+ }
}
-// Prototype"!!!!
-void speedControl(int);
-
+//Brake code
void brakeControl(int brakeRate) {
- if (driveMode == 2) { // PARK MODE
- // BREAK RATE LEVEL 1
- speedControl(0);
- brakeValve32 = 0;
- brakeValve22 = 1;
- //if (pressureSwitch1 == 0) {
-// brakeValve22 = 0;
-// }
-// else {
-// brakeValve22 = 1;
-// }
+ if (driveMode == 2) { // PARK MODE
+ // All Mechanical brakes applied
+ motor1.throttle(0.0f);
+ brakeValve32 = 0;
+ brakeValve22 = 0;
+ }
+ else {//REGEN BRAKING
+ if (challenge.regenBrakingActive == true) { // REGEN BRAKING WITH OVERVOLTAGE SAFETY CHECK
+ if (brakeRate > 0) {
+ motor1.setPark();
+ }
+ else {
+ motor1.setForward();
+ }
}
- else {
-
- if (challenge.regenBrakingActive == true) { // REGEN BRAKING WITH OVERVOLTAGE SAFETY CHECK
- if (brakeRate > 0) {
- motor1.setPark();
- }
- else {
- motor1.setForward();
- }
- // switch (brakeRate) {
-// case 0: // NO BRAKING
-// motor1.brake(0.00f);
-//// motor2.brake(0.00f);
-//// contactBatt = 0; // Close battery contactor so all power comes from supercaps
-// pc.printf("Regen Braking set to 0%\r\n");
-// break;
-//
-// case 1:
-// motor1.throttle(0.0f);
-// motor1.brake(0.33f);
-//// motor2.brake(0.33f);
-//// contactBatt = 0; // Open battery contactor so all power comes from supercaps
-// pc.printf("Regen Braking set to 33%\r\n");
-// break;
-//
-// case 2:
-// motor1.brake(0.66f);
-// motor1.throttle(0.0f);
-//// motor2.brake(0.66f);
-//// contactBatt = 0; // Open battery contactor so all power comes from supercaps
-// pc.printf("Regen Braking set to 66%\r\n");
-// break;
-//
-// case 3:
-// motor1.brake(1.0f);
-// motor1.throttle(0.0f);
-//// motor2.brake(1.0f);
-// pc.printf("Regen Braking set to 100%\r\n");
-// break;
-//
-// default:
-// break;
-// }
- }
- else { // MECHANICAL BRAKING
- switch (brakeRate) {
- case 0: // NO BRAKING
- brakeValve32 = 1;//(PF_2)
- brakeValve22 = 1;//(PG_1)
- break;
-
- case 1: //One brake high
- motor1.throttle(0.0f);
- brakeValve32 = 0;//(PF_2)
- brakeValve22 = 1;//(PG_1)
- break;
- case 2 ... 4 : //Two brake high
- motor1.throttle(0.0f);
- brakeValve32 = 0;//(PF_2)
- brakeValve22 = 0;//(PG_1)
- break;
-
+ else { // MECHANICAL BRAKING
+ switch (brakeRate) {
+ case 0: // NO BRAKING
+ brakeValve32 = 1;//(PF_2)
+ brakeValve22 = 1;//(PG_1)
+ break;
+
+ case 1: //HALF BRAKING
+ motor1.throttle(0.0f);
+ brakeValve32 = 0;//(PF_2)
+ brakeValve22 = 1;//(PG_1)
+ break;
-// case 1:
-// motor1.throttle(0.0f);
-// brakeValve32 = 0;
-// if (pressureSwitch1.read() == 0) {
-// brakeValve22 = 0;
-// pc.printf("Pressure 1 Reached");
-// }
-// else {
-// brakeValve22 = 1;
-// pc.printf("Braking Level 1\r\n");
-// }
-// break;
-
-// case 2:
-// motor1.throttle(0.0f);
-// brakeValve32 = 0;
-// if (pressureSwitch2.read() == 0) {
-// brakeValve22 = 0;
-// pc.printf("Pressure 2 Reached");
-// }
-// else {
-// brakeValve22 = 1;
-// pc.printf("Braking Level 2\r\n");
-// }
-//
-// break;
-//
-// case 2 ... 4:
-// motor1.throttle(0.0f);
-// brakeValve32 = 0;
-// brakeValve22 = 1;
-
-// if (pressureSwitch3.read() == 0) {
-// brakeValve22 = 0;
-// pc.printf("Pressure 3 Reached");
-// }
-// else {
-// brakeValve22 = 1;
-// pc.printf("Braking Level 3\r\n");
-// }
-// break;
-
-
- default: // NO BRAKING
- brakeValve32 = 1;
- brakeValve22 = 1;
- break;
- }
- }
- }
- return;
+ case 2 ... 4 : //FULL BRAKING
+ motor1.throttle(0.0f);
+ brakeValve32 = 0;//(PF_2)
+ brakeValve22 = 0;//(PG_1)
+ break;
+
+ default: // NO BRAKING
+ brakeValve32 = 1;
+ brakeValve22 = 1;
+ break;
+ }
+ }
+ }
+ return;
}
+//Motor code
void speedControl(int commandedSpeed) {
- if (dashboard.currentSpeed < 16.00) { // IF SPEED LESS THAN LIMIT
- switch (commandedSpeed) {
-
- default:
-// motor1.throttle(0.0f);
- break;
-
- case 0:
- motor1.throttle(0.0f);
- break;
-
- case 1 ... 2:
- motor1.throttle(0.1f);
- break;
-
- case 3 ... 4:
- motor1.throttle(0.2f);
- break;
-
- case 5 ... 6:
- motor1.throttle(0.3f);
- break;
-
- case 7 ... 8:
- motor1.throttle(0.4f);
- break;
-
- case 9 ... 10:
- motor1.throttle(0.5f);
- break;
-
- case 11:
- motor1.throttle(0.6f);
- break;
-
- case 12:
- motor1.throttle(0.7f);
- break;
-
- case 13:
- motor1.throttle(0.8f);
- break;
-
- case 14:
- motor1.throttle(0.9f);
- break;
-
- case 15:
- motor1.throttle(1.0f);
- break;
- }
- }
- else { // IF OVER 15KPH
- 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
- motor1.throttle(0.0f); // COMMENTED AS ALREADY SET 0 IN BRAKECONTROL
-// brakeControl(1);
- }
- }
+ switch (commandedSpeed) {
+
+ default:
+ break;
+
+ case 0:
+ motor1.throttle(0.0f);
+ break;
+
+ case 1 ... 2:
+ motor1.throttle(0.1f);
+ break;
+
+ case 3 ... 4:
+ motor1.throttle(0.2f);
+ break;
+
+ case 5 ... 6:
+ motor1.throttle(0.3f);
+ break;
+
+ case 7 ... 8:
+ motor1.throttle(0.4f);
+ break;
+
+ case 9 ... 10:
+ motor1.throttle(0.5f);
+ break;
+
+ case 11:
+ motor1.throttle(0.6f);
+ break;
+
+ case 12:
+ motor1.throttle(0.7f);
+ break;
+
+ case 13:
+ motor1.throttle(0.8f);
+ break;
+
+ case 14:
+ motor1.throttle(0.9f);
+ break;
+
+ case 15:
+ motor1.throttle(1.0f);
+ break;
+ }
}
-/*void speedControl(int commandedSpeed) {
- if (commandedSpeed > dashboard.currentSpeed) { // IF THROTTLE REQUESTED
- // Max possible difference is 15
- // Motor Analog Voltage between 0 and 5
- // 5 / 15 = 0.33333 = 0.4v / kph difference
-
- int difference = commandedSpeed - dashboard.currentSpeed;
-
- switch (difference) {
- case 1:
- motor1.throttle(0.1f);
-// motor2.throttle(0.1f);
- pc.printf("Throttle set to 10%\r\n");
- break;
-
- case 2 ... 3:
- motor1.throttle(0.2f);
-// motor2.throttle(0.2f);
- pc.printf("Throttle set to 20%\r\n");
- break;
-
- case 4 ... 6:
- motor1.throttle(0.4f);
-// motor2.throttle(0.4f);
- pc.printf("Throttle set to 40%\r\n");
- break;
-
- case 7 ... 9:
- motor1.throttle(0.6f);
-// motor2.throttle(0.6f);
- pc.printf("Throttle set to 60%\r\n");
- break;
-
- case 10 ... 12:
- motor1.throttle(0.8f);
-// motor2.throttle(0.8f);
- pc.printf("Throttle set to 80%\r\n");
- break;
-
- case 13 ... 15:
- motor1.throttle(1.0f);
-// motor2.throttle(1.0f);
- pc.printf("Throttle set to 100%\r\n");
- break;
-
- default:
- motor1.throttle(0.0f);
- break;
+int main() {
+ pc.baud(115200);
+
+ // CONFIGURE INTERRUPTS
+ rtc_output.rise(&emergencyStop);
+
+ millisStart();
+
+ rtc_Trigger = 1;
+
+ // LOCAL VARIABLES
+ bool systemOn = false; // On/Off status of loco
+ int lastKnownDirection = 2;
+ bool inParkMode = false;
+
+ // Record last time error was sent
+ unsigned long lastErrorMillis = 0;
+
+ //MainLoop
+ while (1) {
+
+ while (remote.commsGood == true) {
+ /////Start Up///////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /////Checking Modes from controller/////////////////////////////////////////////////////////////////////////////////
+ // PING
+ remote.commsCheck();
+
+ // GET SWITCH STATES
+ remote.getSwitchStates();
+
+ // ALLOW BRAKES TO BE OPERATED
+ brakeControl(remote.braking);
+
+ // SOUND WHISTLE IF WHISTLE BUTTON PRESSED
+ if (remote.whistle == 0) {
+ whistleValve32 = 1;
+ }
+ else {
+ whistleValve32 = 0;
+ }
+
+ // GET AND DISPLAY SPEED
+ dashboard.getCurrentSpeed();
+ remote.sendData(2, dashboard.currentSpeed); // Send speed to remote
+
+ // TOGGLE COMPRESSOR
+ remote.compressor == 0 ? contactCompressor = 1 : contactCompressor = 0;
+
+ // TOGGLE MOTOR CONTROLLER DEADMAN (SEAT SWITCH AND FOOTBRAKE)
+ if (rtc.deadman == 0) { // IF DEADMAN PRESSED
+ motor1.closeDeadman();
+ }
+ else {
+ motor1.releaseDeadman();
+ }
+
+ // TOGGLE REGEN THROTTLING
+ if (challenge.regenThrottleActive == false) {
+ if (remote.regenThrottle == 0 && remote.start == 0) { // TURN OFF IF ON
+ challenge.regenThrottleOn();
}
- }
- else { // COAST
- motor1.throttle(0.0f);
-// motor2.throttle(0.0f);
- }
-}*/
+ }
+ else {
+ remote.sendError('B'); // Send error to remote
+ if (remote.regenThrottle == 1) { // TURN ON IF OFF
+ challenge.regenThrottleOff();
+ }
+ }
-int main() {
- pc.baud(115200);
-
- // CONFIGURE INTERRUPTS
- rtc_output.rise(&emergencyStop);
-
- millisStart();
-
- rtc_Trigger = 1;
-
- // LOCAL VARIABLES
- bool systemOn = false; // On/Off status of loco
- int lastKnownDirection = 2;
- bool inParkMode = false;
-
- // Record last time error was sent
- unsigned long lastErrorMillis = 0;
+ // TOGGLE REGEN BRAKING
+ if (challenge.regenBrakingActive == false) {
+ if (remote.regenBrake == 0 && remote.start == 0) { // TURN OFF IF ON
+ if (challenge.regenBrakingOn() == 0) {
+ remote.sendError('I'); // Send error to remote
+ pc.printf("Regen Braking Off - SuperCaps are full\r\n");
+ }
+ }
+ }
+ else {
+ remote.sendError('C'); // Send error to remote
+ if (remote.regenBrake == 1) { // TURN OFF
+ challenge.regenBrakingOff();
+ if (superCapVoltage == 1) {
+ pc.printf("Regen Braking Off - SuperCaps are full\r\n");
+ remote.sendError('I'); // Send error to remote
+ }
+ }
+ }
- while(1) {
-
-
- while(remote.commsGood == true) {
- DisplaySerial();//Datalogger Chai Funciton
-
- // PING
- remote.commsCheck();
- // GET SWITCH STATES
- remote.getSwitchStates();
-
- // ALLOW BRAKES TO BE OPERATED
- brakeControl(remote.braking);
-
- // Print Pressure Switch States (Debugging)
-// pc.printf("Pressure Switch 1: %d\r\n", pressureSwitch1.read());
-// pc.printf("Pressure Switch 2: %d\r\n", pressureSwitch2.read());
-// pc.printf("Pressure Switch 3: %d\r\n", pressureSwitch3.read());
-
- // SOUND WHISTLE IF WHISTLE BUTTON PRESSED
- if (remote.whistle == 0) {
- whistleValve32 = 1;
-// wait(0.5);
-// whistleValve32 = 1;
- }
- else {
- whistleValve32 = 0;
- }
+ // TOGGLE AUTOSTOP
+ if (challenge.autoStopActive == 0) {
+ if (remote.autoStop == 0 && remote.start == 0) { // TURN OFF IF ON
+ challenge.autoStopOn();
+ }
+ }
+ else {
+ remote.sendError('D'); // Send error to remote
+ if (remote.autoStop == 1) { // TURN ON IF OFF
+ challenge.autoStopOff();
+ }
+ }
+ // TOGGLE INNOVATION
+ if (challenge.innovationActive == 0) {
+ if (remote.innovation == 0 && remote.start == 0) { // TURN OFF IF ON
+ if (driveMode == 0) {
+ challenge.innovationOn();
+ }
+ else {
+ remote.sendError('J'); // Send error to remote
+ pc.printf("Can only active innovation mode in forward direction\r\n");
+ }
+ }
+ }
+ else {
+ remote.sendError('E'); // Send error to remote
+
+ if (remote.innovation == 1) { // TURN ON IF OFF
+ challenge.innovationOff();
+ }
+ }
+ /////END OF TOGGLE CHECKS//////////////////////////////////////////////////////////////////////////////////////////////
- // GET AND DISPLAY SPEED
- dashboard.getCurrentSpeed();
- remote.sendData(2, dashboard.currentSpeed); // Send speed to remote
-
- // TOGGLE COMPRESSOR
- remote.compressor == 0 ? contactCompressor = 1 : contactCompressor = 0;
-
- // TOGGLE MOTOR CONTROLLER DEADMAN (SEAT SWITCH AND FOOTBRAKE)
- if (rtc.deadman == 0) { // IF DEADMAN PRESSED
- motor1.closeDeadman();
- }
- else {
- motor1.releaseDeadman();
- }
-
- // TOGGLE REGEN THROTTLING
- if (challenge.regenThrottleActive == false) {
- if (remote.regenThrottle == 0 && remote.start == 0) { // TURN OFF IF ON
- challenge.regenThrottleOn();
- }
- }
- else {
- remote.sendError('B'); // Send error to remote
- if (remote.regenThrottle == 1) { // TURN ON IF OFF
- challenge.regenThrottleOff();
- }
- }
-
- // TOGGLE REGEN BRAKING
- if (challenge.regenBrakingActive == false) {
- if (remote.regenBrake == 0 && remote.start == 0) { // TURN OFF IF ON
- if (challenge.regenBrakingOn() == 0) {
- remote.sendError('I'); // Send error to remote
- pc.printf("Regen Braking Off - SuperCaps are full\r\n");
- }
- }
- }
- else {
- remote.sendError('C'); // Send error to remote
- if (remote.regenBrake == 1) { // TURN OFF
- challenge.regenBrakingOff();
- if (superCapVoltage == 1) {
- pc.printf("Regen Braking Off - SuperCaps are full\r\n");
- remote.sendError('I'); // Send error to remote
- }
- }
+ /////Control///////////////////////////////////////////////////////////////////////////////////////////////////////////
+ //Is a Toggle check, but it is the train start swtich A
+ if (systemOn == false) {
+ if (remote.start == 1) {
+ if (millis() - lastErrorMillis > 500) {
+ remote.sendError('A'); // SEND ERROR MESSAGE 'A' TO REMOTE
+ lastErrorMillis = millis();
+ }
+ motor1.turnOff();
+ }
+ else {
+ systemOn = true;
+ motor1.turnOn(); // Turn on motors
+ }
+ } // END IF SYSTEMON = FALSE
+ //If train is switched on and in start do this start///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ else { // IF SYSTEMON == TRUE
+ if (remote.start == 1) {
+ systemOn = false; // WILL STOP ABOVE HERE NEXT LOOP
+ pc.printf("Start Switch is Off\r\n");
+ }
+
+ //Set foward
+ if (driveMode != 0 && remote.forward == 0) {
+ driveMode = 0;
+ motor1.setForward();
+ }
+ //Set reverse
+ if (driveMode != 1 && remote.reverse == 0) {
+ driveMode = 1;
+ motor1.setReverse();
+ }
+ //Set park
+ if (driveMode != 2 && remote.park == 0) {
+ driveMode = 2;
+ motor1.setPark();
+ motor1.throttle(0);
+ }
+ ////Park Mode
+ if (driveMode == 2) { //place in park mode if selected by driver
+ if (inParkMode == false) {
+ pc.printf("Train in park mode.\r\n"); //why?
+ }
+
+ if (emergencyStopActive == true && rtc_output.read() == 0) { // Clear emergency stop flag
+ emergencyStopActive = false;
+ }
+
+ led_parkMode = 1;
+ inParkMode = true; // Stop above debug print from displaying more than once
+ motor1.setPark();
+ }
+
+ ////Drive
+ else { //else i.e if selected drive mode is forward or reverse
+ ////////////////// Start of check for error G RTC clear
+ if (emergencyStopActive == false && rtc_output.read() == 0) { // IF RTC FLAGGED AS SAFE
+
+ led_parkMode = 0;
+ inParkMode = false;
+
+ if (driveMode != lastKnownDirection) {
+ pc.printf("Train enabled for direction %d\r\n", driveMode);
+
+ lastKnownDirection = driveMode;
}
- // TOGGLE AUTOSTOP
- if (challenge.autoStopActive == 0) {
- if (remote.autoStop == 0 && remote.start == 0) { // TURN OFF IF ON
- challenge.autoStopOn();
- }
+ ////Call autostop challenge
+ if (challenge.autoStopInProgress == true) { // IF AUTOSTOPPING, PASS THROTTLE CONTROL TO FUNCTION
+ challenge.autoStopControl();
+ pc.printf("Autostop in Control\r\n");
}
- else {
- remote.sendError('D'); // Send error to remote
- if (remote.autoStop == 1) { // TURN ON IF OFF
- challenge.autoStopOff();
+ //Use controls from remote
+ else { // OTHERWISE INPUT THROTTLE FROM REMOTE
+ if (remote.throttle > 0) { // If joystick pushed upwards = throttle
+ /////////////////////////Innovation braking start
+ if (challenge.innovationActive == true) {
+ pc.printf("Collision Detection in Control\r\n");
+ int innovationThrottle = challenge.innovationControl(remote.throttle);
+ speedControl(innovationThrottle);
+
+ if (innovationThrottle == 0) {
+ emergencyStop(); // emergency Brake if obstacle detected
+ }
}
+ /////////////////////////Innovation braking end
+ //normal throttle control
+ else {
+ speedControl(remote.throttle);
+ pc.printf("Throttling: %d\r\n", remote.throttle);
+ }
+ } // remote.throttle
+ ///if nothing set to 0
+ else {
+ speedControl(0);
+ }
}
- // TOGGLE INNOVATION
- if (challenge.innovationActive == 0) {
- if (remote.innovation == 0 && remote.start == 0) { // TURN OFF IF ON
- if (driveMode == 0) {
- challenge.innovationOn();
- }
- else {
- remote.sendError('J'); // Send error to remote
- pc.printf("Can only active innovation mode in forward direction\r\n");
- }
- }
- }
- else {
- remote.sendError('E'); // Send error to remote
-
- if (remote.innovation == 1) { // TURN ON IF OFF
- challenge.innovationOff();
- }
- }
-
-
- // CONTROL
-
- if (systemOn == false) {
-
- if (remote.start == 1) {
-
- if (millis() - lastErrorMillis > 500) {
- remote.sendError('A'); // SEND ERROR MESSAGE 'A' TO REMOTE
- lastErrorMillis = millis();
- }
-
- motor1.turnOff();
-// motor2.turnOff();
- }
- else {
- systemOn = true;
- pc.printf("Start Switch is On\r\n");
-// startupHealthCheck(); // Run System Startup Health Check - Will stay in here until it passes
-
- motor1.turnOn(); // Turn on motors
-// motor2.turnOn();
-
- }
- } // END IF SYSTEMON = FALSE
- else { // IF SYSTEMON == TRUE
- if (remote.start == 1) {
- systemOn = false; // WILL STOP ABOVE HERE NEXT LOOP
- pc.printf("Start Switch is Off\r\n");
- }
-
- if (driveMode != 0 && remote.forward == 0) {
- driveMode = 0;
- motor1.setForward();
-// motor2.setForward();
- }
- if (driveMode != 1 && remote.reverse == 0) {
- driveMode = 1;
- motor1.setReverse();
-// motor2.setReverse();
- }
- if (driveMode != 2 && remote.park == 0) {
- driveMode = 2;
- motor1.setPark();
- motor1.throttle(0);
-// motor2.setPark();
- }
+ }
+ ////////////////// End of check for error G RTC clear
+ else {
+ pc.printf("Cannot exit park mode until RTC is cleared\r\n");
+ inParkMode = false;
+ remote.sendError('G'); // Send error to remote
+ }
+ }
+ //Datalogger Chai Funciton
+ DisplaySerial();
+ } // END IF (SYSTEMON == TRUE)
+ //If train is switched on and in start do this end///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- if (driveMode == 2) { //place in park mode if selected by driver
-
-// pc.printf("RTC Output is %d\r\n", rtc_output.read());
-// pc.printf("EM State Output is %d\r\n", emergencyStopActive);
-// pc.printf("ParkMode = %d", inParkMode);
-
- if (inParkMode == false) {
- pc.printf("Train in park mode.\r\n"); //why?
- }
-
- if (emergencyStopActive == true && rtc_output.read() == 0) { // Clear emergency stop flag
- emergencyStopActive = false;
- }
-
- led_parkMode = 1;
- inParkMode = true; // Stop above debug print from displaying more than once
-
- motor1.setPark();
-// motor2.setPark();
-
- }
- else{ //else i.e if selected drive mode is forward or reverse
-// pc.printf("RTC Output is %d\r\n", rtc_output.read());
-// pc.printf("EM State Output is %d\r\n", emergencyStopActive);
- if (emergencyStopActive == false && rtc_output.read() == 0) { // IF RTC FLAGGED AS SAFE
-
-// if (dashboard.currentSpeed < 1 || driveMode == lastKnownDirection) { //do not allow motors to reverse if significant forward speed exists
-
- led_parkMode = 0;
- inParkMode = false;
-
- if (driveMode != lastKnownDirection) {
- pc.printf("Train enabled for direction %d\r\n", driveMode);
-
- lastKnownDirection = driveMode;
- }
-
+ wait_ms(500); // SLOW DOWN THE SYSTEM (REMOTE CANT KEEP UP)
+ } // END WHILE(COMMSGOOD)
+ pc.printf("Main Loop Skipped Due To Emergency Status\r\n");
+ emergencyStop(); // Emergency stop if comms lost with remote controller
- if (challenge.autoStopInProgress == true) { // IF AUTOSTOPPING, PASS THROTTLE CONTROL TO FUNCTION
- challenge.autoStopControl();
- pc.printf("Autostop in Control\r\n");
- }
- else { // OTHERWISE INPUT THROTTLE FROM REMOTE
- if (remote.throttle > 0) { // If joystick pushed upwards = throttle
-
- if (challenge.innovationActive == true) {
- pc.printf("Collision Detection in Control\r\n");
- int innovationThrottle = challenge.innovationControl(remote.throttle);
- speedControl(innovationThrottle);
-
- if (innovationThrottle == 0) {
- emergencyStop(); // emergency Brake if obstacle detected
- }
- }
- else {
- speedControl(remote.throttle);
- pc.printf("Throttling: %d\r\n", remote.throttle);
- }
- } // remote.throttle
- else {
- speedControl(0);
- }
- }
-// }
-// else {
-// pc.printf("Cannot change direction until train has stopped moving\r\n");
-// remote.sendError('F'); // Send error to remote
-// }
- }
- else {
- pc.printf("Cannot exit park mode until RTC is cleared\r\n");
- inParkMode = false;
- remote.sendError('G'); // Send error to remote
- }
- }
- } // END IF (SYSTEMON == TRUE)
-
- wait_ms(500); // SLOW DOWN THE SYSTEM (REMOTE CANT KEEP UP)
- } // END WHILE(COMMSGOOD)
- pc.printf("Main Loop Skipped Due To Emergency Status\r\n");
- emergencyStop(); // Emergency stop if comms lost with remote controller
-
- } //END WHILE(1)
-}
\ No newline at end of file
+ } //END WHILE(1)
+}