Railway Challenge
Uncommenting of part that allow supercaps to charge up from the batteries
Diff: main.cpp
- Revision:
- 0:4788e1df7b55
- Child:
- 1:ba85dae98035
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Mar 13 11:48:04 2020 +0000
@@ -0,0 +1,575 @@
+#include <mbed.h>
+#include "millis.h"
+
+#include "definitions.h"
+#include "remoteControl.h"
+#include "dashboard.h"
+#include "rtc.h"
+#include "motor.h"
+#include "challenge.h"
+
+// SET UP REMOTE CONTROL COMMS
+SPI remoteControl(PE_14, PE_13, PE_12); // (SPI_MOSI, SPI_MISO, SPI_SCK)
+DigitalOut remoteControlCS(PE_11); // (SPI_SS)
+
+// CREATE OBJECTS
+Remote remote(remoteControl, remoteControlCS);
+Dashboard dashboard(hallSensor);
+RoundTrainCircuit rtc(rtc_1, rtc_2, rtc_3, rtc_4, rtc_5, rtc_6, rtc_7, rtc_override);
+Motor motor1(motorAccelerator, motorBrake, keySwitchM1, directionFwd, directionRev, footswitchM1, seatM1, inchFwdM1, speedLimit2M1, speedLimit3M1);
+ChallengeMode challenge(autoStopTrigger, dashboard, remote, motor1);
+
+int driveMode = 2; // Drive mode - fwd(0), rev(1), park(2)
+bool emergencyStopActive = false;
+
+// FUNCTIONS
+
+void startupHealthCheck() {
+
+ while(1) {
+
+ 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
+ }
+}
+
+void emergencyStop() {
+// pc.printf("Emergency Stop Active\r\n");
+ if (emergencyStopActive == false) {
+
+ emergencyStopActive = true;
+
+ motor1.disengage(); // Disengage both motors
+// motor2.disengage();
+
+ motor1.setPark(); // Clear Motor Directions
+// motor2.setPark();
+
+ if (rtc_output.read() == 1) {
+ rtc.getTriggerCause(); // Get RTC input status
+ }
+ }
+}
+
+// Prototype"!!!!
+void speedControl(int);
+
+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;
+// }
+ }
+ 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;
+ brakeValve22 = 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;
+}
+
+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);
+ }
+ }
+}
+
+/*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;
+ }
+ }
+ else { // COAST
+ motor1.throttle(0.0f);
+// motor2.throttle(0.0f);
+ }
+}*/
+
+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;
+
+ while(1) {
+
+ while(remote.commsGood == true) {
+
+ // 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;
+ }
+
+
+ // 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
+ }
+ }
+ }
+
+
+ // 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();
+ }
+ }
+
+
+ // 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();
+ }
+
+ 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");
+ }
+
+ 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;
+ }
+
+
+ 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