Railway Challenge / Mbed 2 deprecated challenge

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

Dependencies:   mbed millis

main.cpp@14:aaa46855f00a, 2022-04-27 (annotated)

Committer:
cdevarakonda
Date:
Wed Apr 27 11:17:56 2022 +0000
Revision:
14:aaa46855f00a
Parent:
11:fcf00823f24a
Child:
15:4976d145fbd9
Added a state where the current speed and set throttle speed are both zero for the brakes to be on high (i.e. the train must be stationary and will remain that way) It is left commented out for now

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