Railway Challenge / Mbed OS challenge-2022Fork_speedcontrollimit

Tachonoise filter code added

Dependencies:   millis

main.cpp@38:5e9b8e8d1538, 22 months ago (annotated)

Committer:
edizselay
Date:
Fri Nov 04 15:04:02 2022 +0000
Revision:
38:5e9b8e8d1538
Parent:
36:af4fb5f4988a 
Removed mbed.bld

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