Railway Challenge / Mbed 2 deprecated challenge-ChaiUpdated-AutostopTest-30Throttle

This is a test from 30%Throttle to stop, using full service brake

Dependencies:   mbed millis

main.cpp@8:0fe9f7bde2f9, 2022-04-20 (annotated)

Committer:
jamesmcildowietfl
Date:
Wed Apr 20 15:50:23 2022 +0000
Revision:
8:0fe9f7bde2f9
Parent:
6:d0ba4c659e9c
Child:
9:d1998035418b
Cleaned up code so its easier to read ; Removed all unused functions; Added comments; Added new parking brakes; Changed position of Chai's datalogger code; Changed Emergency stop function so motor=0 before disengage , so final signal is 0 throttle

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