Project Systems Testing team 4 / Mbed 2 deprecated train_control

version for testing not implemented: delta loop safety, power surge, calibration mode

Dependencies:   mbed TextLCD

main.cpp@3:7cfbf73d6809, 2019-06-27 (annotated)

Committer:
KoenKahlman
Date:
Thu Jun 27 09:26:40 2019 +0000
Revision:
3:7cfbf73d6809
Parent:
2:bb96eba66e78 
ignores power surge on startup

Who changed what in which revision?

UserRevisionLine numberNew contents of line
KoenKahlman 0:9c82986d7cb9 1 #include "mbed.h"
KoenKahlman 0:9c82986d7cb9 2 #include <string>
KoenKahlman 0:9c82986d7cb9 3 #include <stdarg.h>
KoenKahlman 0:9c82986d7cb9 4 #include "TextLCD.h"
KoenKahlman 0:9c82986d7cb9 5 #include "MCP23017.h"
KoenKahlman 0:9c82986d7cb9 6 #include "math.h"
KoenKahlman 0:9c82986d7cb9 7 #include <list>
KoenKahlman 0:9c82986d7cb9 8
KoenKahlman 1:1648ce825c31 9 const bool test = false;
KoenKahlman 0:9c82986d7cb9 10 int iter;
KoenKahlman 0:9c82986d7cb9 11 std::list<int> passedMarkers;
KoenKahlman 0:9c82986d7cb9 12 std::list<int>::iterator it;
KoenKahlman 1:1648ce825c31 13 int speedsetting;
KoenKahlman 0:9c82986d7cb9 14
KoenKahlman 1:1648ce825c31 15 //BOX CONNECTIONS
KoenKahlman 0:9c82986d7cb9 16 DigitalIn button1 (p8);
KoenKahlman 0:9c82986d7cb9 17 DigitalIn button2 (p9);
KoenKahlman 0:9c82986d7cb9 18 DigitalIn button3 (p10);
KoenKahlman 0:9c82986d7cb9 19 DigitalIn button4 (p30);
KoenKahlman 0:9c82986d7cb9 20 AnalogIn potentio (p15); // configures pin15 for analog input. Creates object Ain. It is the potentiometer.
KoenKahlman 0:9c82986d7cb9 21 TextLCD lcd(p22, p21, p23, p24, p25, p26); // configures the LCD pins. Creates object lcd
KoenKahlman 0:9c82986d7cb9 22 DigitalOut led1(LED1);
KoenKahlman 0:9c82986d7cb9 23 DigitalOut led2(LED2);
KoenKahlman 0:9c82986d7cb9 24 DigitalOut led3(LED3);
KoenKahlman 1:1648ce825c31 25 DigitalOut led4(LED4);
KoenKahlman 1:1648ce825c31 26 DigitalOut boxled1 (p19);
KoenKahlman 1:1648ce825c31 27 DigitalOut boxled2 (p20);
KoenKahlman 1:1648ce825c31 28 DigitalOut buzzer (p29);
KoenKahlman 0:9c82986d7cb9 29 Serial pc(USBTX, USBRX); // Initialise the serial connection for terminal output
KoenKahlman 0:9c82986d7cb9 30 InterruptIn int0(p13);
KoenKahlman 0:9c82986d7cb9 31 InterruptIn int1(p14);
KoenKahlman 0:9c82986d7cb9 32 I2C i2c(p28, p27);
KoenKahlman 0:9c82986d7cb9 33 MCP23017 *mcp; //hall detectors
KoenKahlman 0:9c82986d7cb9 34 DigitalOut data(p16); // configures pin16 for digital output. Creates object train. This signal should go to the booster
KoenKahlman 0:9c82986d7cb9 35 DigitalOut enable(p17); //configures pin 17 for digital output. creates object enable, this enables the train track or disables it in case of emergency
KoenKahlman 0:9c82986d7cb9 36
KoenKahlman 1:1648ce825c31 37 //CONSTANTS
KoenKahlman 1:1648ce825c31 38 const unsigned int DCCaddress_train1 = 0x01;
KoenKahlman 1:1648ce825c31 39 const unsigned int DCCaddress_train2 = 0x02;
KoenKahlman 1:1648ce825c31 40 const unsigned int DCCaddress_decoder = 0x06;
KoenKahlman 1:1648ce825c31 41 const unsigned int DCCinst_stop = 0x70; //stop 01 1 1 0000
KoenKahlman 1:1648ce825c31 42 const unsigned int DCCinst_estop = 0x71; //01 1 1 0001
KoenKahlman 1:1648ce825c31 43 const unsigned int DCCswitchneutral = 0b10000000;
KoenKahlman 1:1648ce825c31 44 const unsigned int DCCswitch1 = 0b10000001;
KoenKahlman 1:1648ce825c31 45 const unsigned int DCCswitch2 = 0b10000010;
KoenKahlman 1:1648ce825c31 46 const unsigned int DCCswitch3 = 0b10000100;
KoenKahlman 1:1648ce825c31 47 const unsigned int DCCswitch4 = 0b10001000;
KoenKahlman 1:1648ce825c31 48 const double stationWaitTime = 5.;
KoenKahlman 1:1648ce825c31 49 const double switchsafety = 4.; //contant time limit between uses of same switch
KoenKahlman 1:1648ce825c31 50 const int switchwait = 1500; //time inbetween different switch commands
KoenKahlman 1:1648ce825c31 51
KoenKahlman 0:9c82986d7cb9 52 //interrupt flags
KoenKahlman 0:9c82986d7cb9 53 bool int0flag;
KoenKahlman 0:9c82986d7cb9 54 bool int1flag;
KoenKahlman 0:9c82986d7cb9 55
KoenKahlman 0:9c82986d7cb9 56 //global variables for train1
KoenKahlman 0:9c82986d7cb9 57 bool dir1;
KoenKahlman 0:9c82986d7cb9 58 bool light1;
KoenKahlman 0:9c82986d7cb9 59 int speed1;
KoenKahlman 0:9c82986d7cb9 60 int prev1;
KoenKahlman 0:9c82986d7cb9 61 int curr1;
KoenKahlman 0:9c82986d7cb9 62 int next1;
KoenKahlman 1:1648ce825c31 63 Timer station1;
KoenKahlman 1:1648ce825c31 64 bool boarding1;
KoenKahlman 1:1648ce825c31 65 bool command1;
KoenKahlman 1:1648ce825c31 66
KoenKahlman 1:1648ce825c31 67 bool collision;
KoenKahlman 0:9c82986d7cb9 68
KoenKahlman 0:9c82986d7cb9 69 //global variables for train2
KoenKahlman 0:9c82986d7cb9 70 bool dir2;
KoenKahlman 0:9c82986d7cb9 71 bool light2;
KoenKahlman 0:9c82986d7cb9 72 int speed2;
KoenKahlman 0:9c82986d7cb9 73 int prev2;
KoenKahlman 0:9c82986d7cb9 74 int curr2;
KoenKahlman 0:9c82986d7cb9 75 int next2;
KoenKahlman 1:1648ce825c31 76 Timer station2;
KoenKahlman 1:1648ce825c31 77 bool boarding2;
KoenKahlman 1:1648ce825c31 78 bool command2;
KoenKahlman 0:9c82986d7cb9 79
KoenKahlman 0:9c82986d7cb9 80 //global variables for switches
KoenKahlman 0:9c82986d7cb9 81 std::time_t switch1action;
KoenKahlman 0:9c82986d7cb9 82 std::time_t switch2action;
KoenKahlman 0:9c82986d7cb9 83 std::time_t switch3action;
KoenKahlman 0:9c82986d7cb9 84 std::time_t switch4action;
KoenKahlman 0:9c82986d7cb9 85 bool switch1;
KoenKahlman 0:9c82986d7cb9 86 bool switch2;
KoenKahlman 0:9c82986d7cb9 87 bool switch3;
KoenKahlman 0:9c82986d7cb9 88 bool switch4;
KoenKahlman 0:9c82986d7cb9 89
KoenKahlman 0:9c82986d7cb9 90 //FUNCTION PROTOTYPES
KoenKahlman 0:9c82986d7cb9 91 void init_mcp(void);
KoenKahlman 0:9c82986d7cb9 92 void on_int0_change(void);
KoenKahlman 0:9c82986d7cb9 93 void on_int1_change(void);
KoenKahlman 0:9c82986d7cb9 94 std::list<int> circuit0markers(int sensor_data);
KoenKahlman 0:9c82986d7cb9 95 std::list<int> circuit1markers(int sensor_data);
KoenKahlman 0:9c82986d7cb9 96
KoenKahlman 0:9c82986d7cb9 97 void displayTerminalStatus(void);
KoenKahlman 0:9c82986d7cb9 98 void displayLCDstatus(void);
KoenKahlman 0:9c82986d7cb9 99
KoenKahlman 0:9c82986d7cb9 100 void init(void);
KoenKahlman 0:9c82986d7cb9 101 void initSwitches(void);
KoenKahlman 0:9c82986d7cb9 102 void startupWait(void);
KoenKahlman 0:9c82986d7cb9 103 void emergencyStop(void);
KoenKahlman 0:9c82986d7cb9 104 void handleInt0(void);
KoenKahlman 0:9c82986d7cb9 105 void handleInt1(void);
KoenKahlman 0:9c82986d7cb9 106 void trainCommand(void);
KoenKahlman 1:1648ce825c31 107 void updateTrainStatus(int marker);
KoenKahlman 1:1648ce825c31 108 int predictNextMarker(int prev, int curr);
KoenKahlman 0:9c82986d7cb9 109 void iterAction(void);
KoenKahlman 0:9c82986d7cb9 110
KoenKahlman 0:9c82986d7cb9 111 void changeSwitch1(bool);
KoenKahlman 0:9c82986d7cb9 112 void changeSwitch2(bool);
KoenKahlman 0:9c82986d7cb9 113 void changeSwitch3(bool);
KoenKahlman 0:9c82986d7cb9 114 void changeSwitch4(bool);
KoenKahlman 1:1648ce825c31 115 void updateTrainPredictions(void);
KoenKahlman 0:9c82986d7cb9 116
KoenKahlman 0:9c82986d7cb9 117 unsigned int trainInstruction(bool direction, bool light, int speed);
KoenKahlman 1:1648ce825c31 118 void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count);
KoenKahlman 0:9c82986d7cb9 119
KoenKahlman 1:1648ce825c31 120 void driveTrain(int train);
KoenKahlman 1:1648ce825c31 121 void reverseTrain(int train);
KoenKahlman 1:1648ce825c31 122 void stopTrain(int train);
KoenKahlman 0:9c82986d7cb9 123
KoenKahlman 1:1648ce825c31 124 void atStation2(int train);
KoenKahlman 1:1648ce825c31 125 void atStation12(int train);
KoenKahlman 1:1648ce825c31 126 void atSwitch3(int train);
KoenKahlman 1:1648ce825c31 127 void atSwitch4(int train);
KoenKahlman 1:1648ce825c31 128 void headCollision(void);
KoenKahlman 1:1648ce825c31 129 void sideCollision(void);
KoenKahlman 1:1648ce825c31 130 void xCollision(void);
KoenKahlman 1:1648ce825c31 131 bool isXCollision(void);
KoenKahlman 1:1648ce825c31 132 void tailCollision(int backTrain);
KoenKahlman 1:1648ce825c31 133 void atStation(int train);
KoenKahlman 1:1648ce825c31 134 //void loopDeltaCrossing(int train);
KoenKahlman 0:9c82986d7cb9 135
KoenKahlman 0:9c82986d7cb9 136 int main() {
KoenKahlman 1:1648ce825c31 137 buzzer = 0;
KoenKahlman 0:9c82986d7cb9 138 wait_ms(1000); //safety wait when starting (power surge)
KoenKahlman 1:1648ce825c31 139 enable = false; //track off while doing startup
KoenKahlman 0:9c82986d7cb9 140 while(1){
KoenKahlman 0:9c82986d7cb9 141 init(); // initialize all the things to starting values
KoenKahlman 0:9c82986d7cb9 142 startupWait(); //wait for user to press button 3 to begin
KoenKahlman 0:9c82986d7cb9 143 enable = true;
KoenKahlman 1:1648ce825c31 144 initSwitches(); //put switches in starting position
KoenKahlman 1:1648ce825c31 145 wait_ms(2000); //power surge safety
KoenKahlman 3:7cfbf73d6809 146 if(int0flag || int1flag){
KoenKahlman 3:7cfbf73d6809 147 pc.printf("WARNING: Initialization power surge! Sensor ignored\n\r");
KoenKahlman 3:7cfbf73d6809 148 if(int0flag){
KoenKahlman 3:7cfbf73d6809 149 int trash = mcp->_read(INTCAPA);
KoenKahlman 3:7cfbf73d6809 150 int0flag = false;
KoenKahlman 3:7cfbf73d6809 151 }
KoenKahlman 3:7cfbf73d6809 152 if(int1flag){
KoenKahlman 3:7cfbf73d6809 153 int trash = mcp->_read(INTCAPB);
KoenKahlman 3:7cfbf73d6809 154 int1flag = false;
KoenKahlman 3:7cfbf73d6809 155 }
KoenKahlman 3:7cfbf73d6809 156 }
KoenKahlman 0:9c82986d7cb9 157
KoenKahlman 0:9c82986d7cb9 158 if(!test){
KoenKahlman 0:9c82986d7cb9 159 iter = 0; //iter count
KoenKahlman 0:9c82986d7cb9 160 while(enable){
KoenKahlman 0:9c82986d7cb9 161 if(!button4){
KoenKahlman 0:9c82986d7cb9 162 emergencyStop();
KoenKahlman 0:9c82986d7cb9 163 break;
KoenKahlman 1:1648ce825c31 164 }
KoenKahlman 1:1648ce825c31 165
KoenKahlman 0:9c82986d7cb9 166 if(int0flag){
KoenKahlman 0:9c82986d7cb9 167 handleInt0(); //store passed circuit 0 markers in the list passed markers
KoenKahlman 0:9c82986d7cb9 168 }
KoenKahlman 0:9c82986d7cb9 169
KoenKahlman 0:9c82986d7cb9 170 if(int1flag){
KoenKahlman 0:9c82986d7cb9 171 handleInt1(); //same for circuit 1
KoenKahlman 0:9c82986d7cb9 172 }
KoenKahlman 0:9c82986d7cb9 173
KoenKahlman 1:1648ce825c31 174 if(!passedMarkers.empty()){
KoenKahlman 0:9c82986d7cb9 175 for (it=passedMarkers.begin(); it!=passedMarkers.end(); ++it){
KoenKahlman 1:1648ce825c31 176 if(*it != next1 && *it != next2 && *it != curr1 && *it != curr2){ //TODO check if safe
KoenKahlman 1:1648ce825c31 177 pc.printf("ERROR: Unexpected marker %d. Expected train1: %d or train2: %d. Sensor failure or unknown object on track. Aborting.\n\r", *it, next1, next2);
KoenKahlman 0:9c82986d7cb9 178 enable = false;
KoenKahlman 1:1648ce825c31 179 break;
KoenKahlman 1:1648ce825c31 180 }
KoenKahlman 0:9c82986d7cb9 181 updateTrainStatus(*it);
KoenKahlman 0:9c82986d7cb9 182 }
KoenKahlman 0:9c82986d7cb9 183 passedMarkers.clear();
KoenKahlman 0:9c82986d7cb9 184 }
KoenKahlman 0:9c82986d7cb9 185
KoenKahlman 0:9c82986d7cb9 186 trainCommand(); //decides for a command to send to the track
KoenKahlman 0:9c82986d7cb9 187
KoenKahlman 1:1648ce825c31 188 iterAction(); //every X iterations display some stuff
KoenKahlman 0:9c82986d7cb9 189 iter++;
KoenKahlman 0:9c82986d7cb9 190 }
KoenKahlman 0:9c82986d7cb9 191 } else {
KoenKahlman 0:9c82986d7cb9 192 //test scripts
KoenKahlman 0:9c82986d7cb9 193 }
KoenKahlman 0:9c82986d7cb9 194 }
KoenKahlman 0:9c82986d7cb9 195 }
KoenKahlman 0:9c82986d7cb9 196
KoenKahlman 0:9c82986d7cb9 197 //MAIN LOOP ROUTINES
KoenKahlman 1:1648ce825c31 198 void init(){
KoenKahlman 1:1648ce825c31 199 command1 = false;
KoenKahlman 1:1648ce825c31 200 command2 = false;
KoenKahlman 1:1648ce825c31 201 collision = false;
KoenKahlman 1:1648ce825c31 202
KoenKahlman 0:9c82986d7cb9 203 //initialize clock time
KoenKahlman 0:9c82986d7cb9 204 set_time(1);
KoenKahlman 0:9c82986d7cb9 205 switch1action = time(NULL);
KoenKahlman 0:9c82986d7cb9 206 switch2action = time(NULL);
KoenKahlman 0:9c82986d7cb9 207 switch3action = time(NULL);
KoenKahlman 0:9c82986d7cb9 208 switch4action = time(NULL);
KoenKahlman 0:9c82986d7cb9 209
KoenKahlman 0:9c82986d7cb9 210 led1 = 0;
KoenKahlman 0:9c82986d7cb9 211 led2 = 0;
KoenKahlman 0:9c82986d7cb9 212 led3 = 0;
KoenKahlman 1:1648ce825c31 213 buzzer = 0;
KoenKahlman 1:1648ce825c31 214 boxled1 = 0;
KoenKahlman 1:1648ce825c31 215 boxled2 = 0;
KoenKahlman 1:1648ce825c31 216
KoenKahlman 1:1648ce825c31 217 speedsetting = 7;
KoenKahlman 0:9c82986d7cb9 218
KoenKahlman 0:9c82986d7cb9 219 //pc.baud(921600); //faster printing
KoenKahlman 0:9c82986d7cb9 220 pc.baud(19200);
KoenKahlman 0:9c82986d7cb9 221
KoenKahlman 0:9c82986d7cb9 222 //initialize mcp //(initialize interrupt flags)
KoenKahlman 0:9c82986d7cb9 223 init_mcp();
KoenKahlman 0:9c82986d7cb9 224
KoenKahlman 0:9c82986d7cb9 225 //initialize train globals
KoenKahlman 0:9c82986d7cb9 226 dir1 = true;
KoenKahlman 0:9c82986d7cb9 227 light1 = true;
KoenKahlman 1:1648ce825c31 228 speed1 = speedsetting;
KoenKahlman 1:1648ce825c31 229 boarding1 = false;
KoenKahlman 1:1648ce825c31 230 //train 1 starting position
KoenKahlman 1:1648ce825c31 231 prev1 = 0;
KoenKahlman 1:1648ce825c31 232 curr1 = 1;
KoenKahlman 1:1648ce825c31 233 next1 = predictNextMarker(prev1, curr1); //2
KoenKahlman 0:9c82986d7cb9 234
KoenKahlman 0:9c82986d7cb9 235 //global variables for train2
KoenKahlman 1:1648ce825c31 236 dir2 = true;
KoenKahlman 0:9c82986d7cb9 237 light2 = true;
KoenKahlman 1:1648ce825c31 238 speed2 = speedsetting;
KoenKahlman 1:1648ce825c31 239 boarding2 = false;
KoenKahlman 1:1648ce825c31 240 //train 2 starting position
KoenKahlman 1:1648ce825c31 241 prev2 = 4;
KoenKahlman 1:1648ce825c31 242 curr2 = 6;
KoenKahlman 1:1648ce825c31 243 next2 = predictNextMarker(prev2, curr2); //7
KoenKahlman 0:9c82986d7cb9 244
KoenKahlman 1:1648ce825c31 245 //initialize presumed switch positions
KoenKahlman 0:9c82986d7cb9 246 switch1 = false;
KoenKahlman 0:9c82986d7cb9 247 switch2 = false;
KoenKahlman 0:9c82986d7cb9 248 switch3 = false;
KoenKahlman 0:9c82986d7cb9 249 switch4 = false;
KoenKahlman 1:1648ce825c31 250 }
KoenKahlman 1:1648ce825c31 251
KoenKahlman 1:1648ce825c31 252 void startupWait(){
KoenKahlman 1:1648ce825c31 253 lcd.cls();
KoenKahlman 3:7cfbf73d6809 254 lcd.printf("Press START when ready");
KoenKahlman 3:7cfbf73d6809 255 pc.printf("Press START when ready. Starting position: train 1 between marker 1 and 2, train 2 between marker 6 and 7.\n\r");
KoenKahlman 1:1648ce825c31 256 while(button3){
KoenKahlman 1:1648ce825c31 257 //do nothing
KoenKahlman 1:1648ce825c31 258 }
KoenKahlman 1:1648ce825c31 259 pc.printf("Starting...\n\r");
KoenKahlman 0:9c82986d7cb9 260 }
KoenKahlman 0:9c82986d7cb9 261
KoenKahlman 0:9c82986d7cb9 262 void initSwitches(){
KoenKahlman 0:9c82986d7cb9 263 wait_ms(switchwait);
KoenKahlman 1:1648ce825c31 264 changeSwitch1(switch1);
KoenKahlman 0:9c82986d7cb9 265 wait_ms(switchwait);
KoenKahlman 1:1648ce825c31 266 changeSwitch2(switch2);
KoenKahlman 0:9c82986d7cb9 267 wait_ms(switchwait);
KoenKahlman 1:1648ce825c31 268 changeSwitch3(switch3);
KoenKahlman 0:9c82986d7cb9 269 wait_ms(switchwait);
KoenKahlman 1:1648ce825c31 270 changeSwitch4(switch4);
KoenKahlman 1:1648ce825c31 271 wait_ms(500);
KoenKahlman 0:9c82986d7cb9 272 }
KoenKahlman 0:9c82986d7cb9 273
KoenKahlman 1:1648ce825c31 274 void emergencyStop(){
KoenKahlman 1:1648ce825c31 275 enable = false;
KoenKahlman 1:1648ce825c31 276 pc.printf("Emergency stop! Press button 3 when ready to restart\n\r");
KoenKahlman 1:1648ce825c31 277
KoenKahlman 1:1648ce825c31 278 lcd.cls();
KoenKahlman 1:1648ce825c31 279 lcd.printf("Press START when ready to restart");
KoenKahlman 1:1648ce825c31 280 while(button3){
KoenKahlman 1:1648ce825c31 281 //do nothing
KoenKahlman 0:9c82986d7cb9 282 }
KoenKahlman 0:9c82986d7cb9 283 }
KoenKahlman 0:9c82986d7cb9 284
KoenKahlman 0:9c82986d7cb9 285 void handleInt0(){
KoenKahlman 0:9c82986d7cb9 286 wait_us(2000);
KoenKahlman 0:9c82986d7cb9 287 int sensor_data = mcp->_read(INTCAPA);
KoenKahlman 0:9c82986d7cb9 288 int0flag = false;
KoenKahlman 0:9c82986d7cb9 289 std::list<int> temp = circuit0markers(sensor_data);
KoenKahlman 0:9c82986d7cb9 290 passedMarkers.splice(passedMarkers.end(), temp);
KoenKahlman 0:9c82986d7cb9 291 }
KoenKahlman 0:9c82986d7cb9 292
KoenKahlman 0:9c82986d7cb9 293 void handleInt1(){
KoenKahlman 0:9c82986d7cb9 294 wait_us(2000);
KoenKahlman 0:9c82986d7cb9 295 int sensor_data = mcp->_read(INTCAPB);
KoenKahlman 0:9c82986d7cb9 296 int1flag = false;
KoenKahlman 0:9c82986d7cb9 297 std::list<int> temp = circuit1markers(sensor_data);
KoenKahlman 0:9c82986d7cb9 298 passedMarkers.splice(passedMarkers.end(), temp);
KoenKahlman 0:9c82986d7cb9 299 }
KoenKahlman 0:9c82986d7cb9 300
KoenKahlman 1:1648ce825c31 301 void updateTrainStatus(int marker){
KoenKahlman 1:1648ce825c31 302 if(marker == 21){
KoenKahlman 1:1648ce825c31 303 return; //marker 21 is unused
KoenKahlman 1:1648ce825c31 304 }
KoenKahlman 1:1648ce825c31 305
KoenKahlman 1:1648ce825c31 306 if(marker == next1){
KoenKahlman 1:1648ce825c31 307 prev1 = curr1;
KoenKahlman 1:1648ce825c31 308 curr1 = next1;
KoenKahlman 1:1648ce825c31 309 next1 = predictNextMarker(prev1, curr1);
KoenKahlman 1:1648ce825c31 310 command1 = false;
KoenKahlman 1:1648ce825c31 311 collision = false;
KoenKahlman 1:1648ce825c31 312 pc.printf("STATUS: Train 1 reached marker %d from marker %d. Predicted marker %d\n\r", curr1, prev1, next1);
KoenKahlman 1:1648ce825c31 313 } else { //marker == next2
KoenKahlman 1:1648ce825c31 314 prev2 = curr2;
KoenKahlman 1:1648ce825c31 315 curr2 = next2;
KoenKahlman 1:1648ce825c31 316 next2 = predictNextMarker(prev2, curr2);
KoenKahlman 1:1648ce825c31 317 command2 = false;
KoenKahlman 1:1648ce825c31 318 collision = false;
KoenKahlman 1:1648ce825c31 319 pc.printf("STATUS: Train 2 reached marker %d from marker %d. Predicted marker %d\n\r", curr2, prev2, next2);
KoenKahlman 1:1648ce825c31 320 }
KoenKahlman 1:1648ce825c31 321 }
KoenKahlman 1:1648ce825c31 322
KoenKahlman 1:1648ce825c31 323 int predictNextMarker(int prev, int curr){
KoenKahlman 1:1648ce825c31 324 switch(prev){
KoenKahlman 1:1648ce825c31 325 case 0: switch(curr){
KoenKahlman 1:1648ce825c31 326 case 1: return 2;
KoenKahlman 1:1648ce825c31 327 case 13: return 12;
KoenKahlman 1:1648ce825c31 328 }
KoenKahlman 1:1648ce825c31 329 case 1: switch(curr){
KoenKahlman 1:1648ce825c31 330 case 2: return switch2?3:4;
KoenKahlman 1:1648ce825c31 331 case 0: return 13;
KoenKahlman 1:1648ce825c31 332 }
KoenKahlman 1:1648ce825c31 333 case 2: switch(curr){
KoenKahlman 1:1648ce825c31 334 case 3: return 9;
KoenKahlman 1:1648ce825c31 335 case 4: return 6;
KoenKahlman 1:1648ce825c31 336 case 1: return 0;
KoenKahlman 1:1648ce825c31 337 }
KoenKahlman 1:1648ce825c31 338 case 3: switch(curr){
KoenKahlman 1:1648ce825c31 339 case 9: return 8;
KoenKahlman 1:1648ce825c31 340 case 2: return 1;
KoenKahlman 1:1648ce825c31 341 }
KoenKahlman 1:1648ce825c31 342 case 4: switch(curr){
KoenKahlman 1:1648ce825c31 343 case 6: return 7;
KoenKahlman 1:1648ce825c31 344 case 2: return 1;
KoenKahlman 1:1648ce825c31 345 }
KoenKahlman 1:1648ce825c31 346 case 5: switch(curr){
KoenKahlman 1:1648ce825c31 347 case 6: return 7;
KoenKahlman 1:1648ce825c31 348 case 11: return 12;
KoenKahlman 1:1648ce825c31 349 }
KoenKahlman 1:1648ce825c31 350 case 6: switch(curr){
KoenKahlman 1:1648ce825c31 351 case 4: return 2;
KoenKahlman 1:1648ce825c31 352 case 5: return 11;
KoenKahlman 1:1648ce825c31 353 case 7: return 8;
KoenKahlman 1:1648ce825c31 354 }
KoenKahlman 1:1648ce825c31 355 case 7: switch(curr){
KoenKahlman 1:1648ce825c31 356 case 6: return switch3?5:4;
KoenKahlman 1:1648ce825c31 357 case 8: return switch4?9:10;
KoenKahlman 1:1648ce825c31 358 }
KoenKahlman 1:1648ce825c31 359 case 8: switch(curr){
KoenKahlman 1:1648ce825c31 360 case 7: return 6;
KoenKahlman 1:1648ce825c31 361 case 9: return 3;
KoenKahlman 1:1648ce825c31 362 case 10: return 12;
KoenKahlman 1:1648ce825c31 363 }
KoenKahlman 1:1648ce825c31 364 case 9: switch(curr){
KoenKahlman 1:1648ce825c31 365 case 3: return 2;
KoenKahlman 1:1648ce825c31 366 case 8: return 7;
KoenKahlman 1:1648ce825c31 367 }
KoenKahlman 1:1648ce825c31 368 case 10: switch(curr){
KoenKahlman 1:1648ce825c31 369 case 8: return 7;
KoenKahlman 1:1648ce825c31 370 case 12: return 13;
KoenKahlman 1:1648ce825c31 371 }
KoenKahlman 1:1648ce825c31 372 case 11: switch(curr){
KoenKahlman 1:1648ce825c31 373 case 5: return 6;
KoenKahlman 1:1648ce825c31 374 case 12: return 13;
KoenKahlman 1:1648ce825c31 375 }
KoenKahlman 1:1648ce825c31 376 case 12: switch(curr){
KoenKahlman 1:1648ce825c31 377 case 10: return 8;
KoenKahlman 1:1648ce825c31 378 case 11: return 5;
KoenKahlman 1:1648ce825c31 379 case 13: return 0;
KoenKahlman 1:1648ce825c31 380 }
KoenKahlman 1:1648ce825c31 381 case 13: switch(curr){
KoenKahlman 1:1648ce825c31 382 case 0: return 1;
KoenKahlman 1:1648ce825c31 383 case 12: return switch1?11:10;
KoenKahlman 1:1648ce825c31 384 }
KoenKahlman 1:1648ce825c31 385 }
KoenKahlman 1:1648ce825c31 386 return 666; //TODO default
KoenKahlman 1:1648ce825c31 387 }
KoenKahlman 1:1648ce825c31 388
KoenKahlman 1:1648ce825c31 389 void iterAction(){
KoenKahlman 1:1648ce825c31 390 displayLCDstatus();
KoenKahlman 1:1648ce825c31 391 if(iter % 100 == 0){
KoenKahlman 1:1648ce825c31 392 displayTerminalStatus();
KoenKahlman 1:1648ce825c31 393 }
KoenKahlman 1:1648ce825c31 394 }
KoenKahlman 1:1648ce825c31 395
KoenKahlman 1:1648ce825c31 396 //TRAIN COMMAND
KoenKahlman 1:1648ce825c31 397 void trainCommand(){
KoenKahlman 1:1648ce825c31 398 float Vin = potentio.read() * 3.3;
KoenKahlman 1:1648ce825c31 399 speedsetting = floor(Vin/0.22); //0.22 = 3.3/15
KoenKahlman 1:1648ce825c31 400
KoenKahlman 1:1648ce825c31 401 if(boarding1 || boarding2){
KoenKahlman 1:1648ce825c31 402 if(station1.read() > stationWaitTime){ //timer works? TODO
KoenKahlman 1:1648ce825c31 403 boarding1 = false;
KoenKahlman 1:1648ce825c31 404 led1 = 0;
KoenKahlman 1:1648ce825c31 405 station1.stop();
KoenKahlman 1:1648ce825c31 406 }
KoenKahlman 1:1648ce825c31 407
KoenKahlman 1:1648ce825c31 408 if(station2.read() > stationWaitTime){
KoenKahlman 1:1648ce825c31 409 boarding2 = false;
KoenKahlman 1:1648ce825c31 410 led2 = 0;
KoenKahlman 1:1648ce825c31 411 station2.stop();
KoenKahlman 1:1648ce825c31 412 }
KoenKahlman 1:1648ce825c31 413 }
KoenKahlman 1:1648ce825c31 414 speed1 = boarding1?0:speedsetting;
KoenKahlman 1:1648ce825c31 415 speed2 = boarding2?0:speedsetting;
KoenKahlman 1:1648ce825c31 416
KoenKahlman 1:1648ce825c31 417 if(next1 == next2 && !collision){
KoenKahlman 1:1648ce825c31 418 headCollision();
KoenKahlman 1:1648ce825c31 419 collision = true;
KoenKahlman 1:1648ce825c31 420 } else if ((next1 == curr2 || next2 == curr1) && !collision){
KoenKahlman 1:1648ce825c31 421 sideCollision();
KoenKahlman 1:1648ce825c31 422 collision = true;
KoenKahlman 1:1648ce825c31 423 } else if (isXCollision() && !collision){
KoenKahlman 1:1648ce825c31 424 xCollision();
KoenKahlman 1:1648ce825c31 425 collision = true;
KoenKahlman 1:1648ce825c31 426 } else if (next1 == prev2 && !command1){
KoenKahlman 1:1648ce825c31 427 tailCollision(1);
KoenKahlman 1:1648ce825c31 428 command1 = true;
KoenKahlman 1:1648ce825c31 429 } else if (next2 == prev1 && !command1){
KoenKahlman 1:1648ce825c31 430 tailCollision(2);
KoenKahlman 1:1648ce825c31 431 command1 = true;
KoenKahlman 1:1648ce825c31 432 } else if (curr1 == 2 && !command1){
KoenKahlman 1:1648ce825c31 433 atStation2(1);
KoenKahlman 1:1648ce825c31 434 command1 = true;
KoenKahlman 1:1648ce825c31 435 } else if (curr1 == 6 && !command1){
KoenKahlman 1:1648ce825c31 436 atSwitch3(1);
KoenKahlman 1:1648ce825c31 437 command1 = true;
KoenKahlman 1:1648ce825c31 438 } else if (curr1 == 8 && !command1){
KoenKahlman 1:1648ce825c31 439 atSwitch4(1);
KoenKahlman 1:1648ce825c31 440 command1 = true;
KoenKahlman 1:1648ce825c31 441 } else if (curr1 == 12 && !command1){
KoenKahlman 1:1648ce825c31 442 atStation12(1);
KoenKahlman 1:1648ce825c31 443 command1 = true;
KoenKahlman 1:1648ce825c31 444 } else if (curr2 == 2 && !command2){
KoenKahlman 1:1648ce825c31 445 atStation2(1);
KoenKahlman 1:1648ce825c31 446 command2 = true;
KoenKahlman 1:1648ce825c31 447 } else if (curr2 == 6 && !command2){
KoenKahlman 1:1648ce825c31 448 atSwitch3(2);
KoenKahlman 1:1648ce825c31 449 command2 = true;
KoenKahlman 1:1648ce825c31 450 } else if (curr2 == 8 && !command2){
KoenKahlman 1:1648ce825c31 451 atSwitch4(2);
KoenKahlman 1:1648ce825c31 452 command2 = true;
KoenKahlman 1:1648ce825c31 453 } else if (curr2 == 12 && !command2){
KoenKahlman 1:1648ce825c31 454 atStation12(2);
KoenKahlman 1:1648ce825c31 455 command2 = true;
KoenKahlman 1:1648ce825c31 456 }
KoenKahlman 1:1648ce825c31 457
KoenKahlman 1:1648ce825c31 458 driveTrain(2);
KoenKahlman 1:1648ce825c31 459 driveTrain(1);
KoenKahlman 1:1648ce825c31 460
KoenKahlman 1:1648ce825c31 461 /*
KoenKahlman 1:1648ce825c31 462 else if (!command1 && (curr1 == 9 && next1 == 3) || (curr1 == 5 && next1 == 11)){
KoenKahlman 1:1648ce825c31 463 loopDeltaCrossing(1);
KoenKahlman 1:1648ce825c31 464 command1 = true;
KoenKahlman 1:1648ce825c31 465 } else if (!command2 && (curr2 == 9 && next2 == 3) || (curr2 == 5 && next2 == 11)){
KoenKahlman 1:1648ce825c31 466 loopDeltaCrossing(2);
KoenKahlman 1:1648ce825c31 467 command2 = true;
KoenKahlman 1:1648ce825c31 468 }
KoenKahlman 1:1648ce825c31 469 */
KoenKahlman 1:1648ce825c31 470 }
KoenKahlman 1:1648ce825c31 471
KoenKahlman 1:1648ce825c31 472 bool isXCollision(){
KoenKahlman 1:1648ce825c31 473 return (curr1 == 3 && next1 == 9) && (next2 == 5 || next2 == 11)
KoenKahlman 1:1648ce825c31 474 || (curr1 == 5 && next1 == 11) && (next2 == 3 || next2 == 9)
KoenKahlman 1:1648ce825c31 475 || (curr1 == 9 && next1 == 3) && (next2 == 5 || next2 == 11)
KoenKahlman 1:1648ce825c31 476 || (curr1 == 11 && next1 == 5) && (next2 == 3 || next2 == 9)
KoenKahlman 1:1648ce825c31 477 || (curr2 == 3 && next2 == 9) && (next1 == 5 || next1 == 11)
KoenKahlman 1:1648ce825c31 478 || (curr2 == 5 && next2 == 11) && (next1 == 3 || next1 == 9)
KoenKahlman 1:1648ce825c31 479 || (curr2 == 9 && next2 == 3) && (next1 == 5 || next1 == 11)
KoenKahlman 1:1648ce825c31 480 || (curr2 == 11 && next2 == 5) && (next1 == 3 || next1 == 9);
KoenKahlman 1:1648ce825c31 481 }
KoenKahlman 1:1648ce825c31 482
KoenKahlman 1:1648ce825c31 483 void stopTrain(int train){
KoenKahlman 1:1648ce825c31 484 if(train == 1){
KoenKahlman 1:1648ce825c31 485 DCC_send_command(DCCaddress_train1, DCCinst_estop, 10);
KoenKahlman 1:1648ce825c31 486 pc.printf("STATUS: Train 1 stopped\n\r");
KoenKahlman 1:1648ce825c31 487 } else { //train == 2
KoenKahlman 1:1648ce825c31 488 DCC_send_command(DCCaddress_train2, DCCinst_estop, 10);
KoenKahlman 1:1648ce825c31 489 pc.printf("STATUS: Train 2 stopped\n\r");
KoenKahlman 1:1648ce825c31 490 }
KoenKahlman 1:1648ce825c31 491 }
KoenKahlman 1:1648ce825c31 492
KoenKahlman 1:1648ce825c31 493 void reverseTrain(int train){ //always stop before reversing
KoenKahlman 1:1648ce825c31 494 if(train == 1){
KoenKahlman 1:1648ce825c31 495 int stupid;
KoenKahlman 1:1648ce825c31 496 dir1 = !dir1;
KoenKahlman 1:1648ce825c31 497 stupid = next1;
KoenKahlman 1:1648ce825c31 498 next1 = curr1;
KoenKahlman 1:1648ce825c31 499 curr1 = stupid;
KoenKahlman 1:1648ce825c31 500 prev1 = predictNextMarker(curr1, next1);
KoenKahlman 1:1648ce825c31 501 pc.printf("STATUS: Train 1 reversed\n\r");
KoenKahlman 1:1648ce825c31 502 } else { //train == 2
KoenKahlman 1:1648ce825c31 503 int swap;
KoenKahlman 1:1648ce825c31 504 dir2 = !dir2;
KoenKahlman 1:1648ce825c31 505 swap = next2;
KoenKahlman 1:1648ce825c31 506 next2 = curr2;
KoenKahlman 1:1648ce825c31 507 curr2 = swap;
KoenKahlman 1:1648ce825c31 508 prev2 = predictNextMarker(curr2, next2);
KoenKahlman 1:1648ce825c31 509 pc.printf("STATUS: Train 2 reversed\n\r");
KoenKahlman 1:1648ce825c31 510 }
KoenKahlman 1:1648ce825c31 511 }
KoenKahlman 1:1648ce825c31 512
KoenKahlman 1:1648ce825c31 513 void driveTrain(int train){
KoenKahlman 1:1648ce825c31 514 if(train == 1){
KoenKahlman 1:1648ce825c31 515 DCC_send_command(DCCaddress_train1, trainInstruction(dir1, light1, speed1), 25);
KoenKahlman 1:1648ce825c31 516 } else { //train == 2
KoenKahlman 1:1648ce825c31 517 DCC_send_command(DCCaddress_train2, trainInstruction(dir2, light2, speed2), 25);
KoenKahlman 1:1648ce825c31 518 }
KoenKahlman 1:1648ce825c31 519 }
KoenKahlman 1:1648ce825c31 520
KoenKahlman 1:1648ce825c31 521 void atStation2(int train){
KoenKahlman 1:1648ce825c31 522 if(train == 1){
KoenKahlman 1:1648ce825c31 523 if(next1 != 22){
KoenKahlman 1:1648ce825c31 524 pc.printf("STATUS: train 1 reached switch 2\n\r");
KoenKahlman 2:bb96eba66e78 525 std::time_t current = time(NULL);
KoenKahlman 2:bb96eba66e78 526 if(difftime(current, switch2action) < switchsafety){
KoenKahlman 2:bb96eba66e78 527 pc.printf("STATUS: Switch 2 on overheat cooldown!\n\r");
KoenKahlman 2:bb96eba66e78 528 return;
KoenKahlman 2:bb96eba66e78 529 }
KoenKahlman 1:1648ce825c31 530 stopTrain(1);
KoenKahlman 1:1648ce825c31 531 if(curr2 == 6 || next2 == 6){
KoenKahlman 1:1648ce825c31 532 switch2 = true;
KoenKahlman 1:1648ce825c31 533 } else if(curr2 == 9 || next2 == 9){
KoenKahlman 1:1648ce825c31 534 switch2 = false;
KoenKahlman 1:1648ce825c31 535 } else { //other train far away
KoenKahlman 1:1648ce825c31 536 switch2 = rand()%2;
KoenKahlman 1:1648ce825c31 537 }
KoenKahlman 1:1648ce825c31 538 changeSwitch2(switch2);
KoenKahlman 2:bb96eba66e78 539 switch2action = time(NULL);
KoenKahlman 1:1648ce825c31 540 }
KoenKahlman 1:1648ce825c31 541 atStation(1);
KoenKahlman 1:1648ce825c31 542 } else { //train == 2
KoenKahlman 1:1648ce825c31 543 if(next2 != 22){
KoenKahlman 1:1648ce825c31 544 pc.printf("STATUS: train 2 reached switch 2\n\r");
KoenKahlman 2:bb96eba66e78 545 std::time_t current = time(NULL);
KoenKahlman 2:bb96eba66e78 546 if(difftime(current, switch2action) < switchsafety){
KoenKahlman 2:bb96eba66e78 547 pc.printf("STATUS: Switch 2 on overheat cooldown!\n\r");
KoenKahlman 2:bb96eba66e78 548 return;
KoenKahlman 2:bb96eba66e78 549 }
KoenKahlman 1:1648ce825c31 550 stopTrain(2);
KoenKahlman 1:1648ce825c31 551 if(curr1 == 6 || next1 == 6){
KoenKahlman 1:1648ce825c31 552 //switch2 = true;
KoenKahlman 1:1648ce825c31 553 } else if(curr1 == 9 || next1 == 9){
KoenKahlman 1:1648ce825c31 554 //switch2 = false;
KoenKahlman 1:1648ce825c31 555 } else { //other train far away
KoenKahlman 1:1648ce825c31 556 switch2 = rand()%2;
KoenKahlman 1:1648ce825c31 557 }
KoenKahlman 2:bb96eba66e78 558 changeSwitch2(switch2);
KoenKahlman 2:bb96eba66e78 559 switch2action = time(NULL);
KoenKahlman 1:1648ce825c31 560 }
KoenKahlman 1:1648ce825c31 561 atStation(2);
KoenKahlman 1:1648ce825c31 562 }
KoenKahlman 1:1648ce825c31 563 }
KoenKahlman 1:1648ce825c31 564
KoenKahlman 1:1648ce825c31 565 void atStation12(int train){
KoenKahlman 1:1648ce825c31 566 if(train == 1){
KoenKahlman 1:1648ce825c31 567 if(next1 != 22){
KoenKahlman 1:1648ce825c31 568 pc.printf("STATUS: train 1 reached switch 1\n\r");
KoenKahlman 2:bb96eba66e78 569 std::time_t current = time(NULL);
KoenKahlman 2:bb96eba66e78 570 if(difftime(current, switch1action) < switchsafety){
KoenKahlman 2:bb96eba66e78 571 pc.printf("STATUS: Switch 1 on overheat cooldown!\n\r");
KoenKahlman 2:bb96eba66e78 572 return;
KoenKahlman 2:bb96eba66e78 573 }
KoenKahlman 1:1648ce825c31 574 stopTrain(1);
KoenKahlman 1:1648ce825c31 575 if(curr2 == 8 || next2 == 8){
KoenKahlman 1:1648ce825c31 576 //switch1 = true;
KoenKahlman 1:1648ce825c31 577 } else if(curr2 == 5 || next2 == 5){
KoenKahlman 1:1648ce825c31 578 //switch1 = false;
KoenKahlman 1:1648ce825c31 579 } else { //other train far away
KoenKahlman 1:1648ce825c31 580 switch1 = rand()%2;
KoenKahlman 1:1648ce825c31 581 }
KoenKahlman 1:1648ce825c31 582 changeSwitch1(switch1);
KoenKahlman 2:bb96eba66e78 583 switch1action = time(NULL);
KoenKahlman 1:1648ce825c31 584 }
KoenKahlman 1:1648ce825c31 585 atStation(1);
KoenKahlman 1:1648ce825c31 586 } else { //train == 2
KoenKahlman 1:1648ce825c31 587 if(next2 != 22){
KoenKahlman 1:1648ce825c31 588 pc.printf("STATUS: train 2 reached switch 1\n\r");
KoenKahlman 2:bb96eba66e78 589 std::time_t current = time(NULL);
KoenKahlman 2:bb96eba66e78 590 if(difftime(current, switch1action) < switchsafety){
KoenKahlman 2:bb96eba66e78 591 pc.printf("STATUS: Switch 1 on overheat cooldown!\n\r");
KoenKahlman 2:bb96eba66e78 592 return;
KoenKahlman 2:bb96eba66e78 593 }
KoenKahlman 1:1648ce825c31 594 stopTrain(2);
KoenKahlman 1:1648ce825c31 595 if(curr1 == 8 || next1 == 8){
KoenKahlman 1:1648ce825c31 596 //switch1 = true;
KoenKahlman 1:1648ce825c31 597 } else if(curr1 == 5 || next1 == 5){
KoenKahlman 1:1648ce825c31 598 //switch1 = false;
KoenKahlman 1:1648ce825c31 599 } else { //other train far away
KoenKahlman 1:1648ce825c31 600 switch1 = rand()%2;
KoenKahlman 1:1648ce825c31 601 }
KoenKahlman 2:bb96eba66e78 602 changeSwitch1(switch1);
KoenKahlman 2:bb96eba66e78 603 switch1action = time(NULL);
KoenKahlman 1:1648ce825c31 604 }
KoenKahlman 1:1648ce825c31 605 atStation(2);
KoenKahlman 1:1648ce825c31 606 }
KoenKahlman 1:1648ce825c31 607 }
KoenKahlman 1:1648ce825c31 608
KoenKahlman 1:1648ce825c31 609 void atStation(int train){
KoenKahlman 1:1648ce825c31 610 if(train == 1){
KoenKahlman 1:1648ce825c31 611 pc.printf("STATUS: Train 1 reached station!\n\r");
KoenKahlman 1:1648ce825c31 612 boxled1 = 1;
KoenKahlman 1:1648ce825c31 613 speed1 = 0;
KoenKahlman 1:1648ce825c31 614 station1.start();
KoenKahlman 1:1648ce825c31 615 boarding1 = true;
KoenKahlman 1:1648ce825c31 616 } else { //train == 2
KoenKahlman 1:1648ce825c31 617 pc.printf("STATUS: Train 2 reached station!\n\r");
KoenKahlman 1:1648ce825c31 618 boxled2 = 1;
KoenKahlman 1:1648ce825c31 619 speed2 = 0;
KoenKahlman 1:1648ce825c31 620 station2.start();
KoenKahlman 1:1648ce825c31 621 boarding2 = true;
KoenKahlman 1:1648ce825c31 622 }
KoenKahlman 1:1648ce825c31 623 }
KoenKahlman 1:1648ce825c31 624
KoenKahlman 1:1648ce825c31 625 void atSwitch3(int train){
KoenKahlman 1:1648ce825c31 626 if(train == 1){
KoenKahlman 1:1648ce825c31 627 if(next1 == 7){ return; }
KoenKahlman 1:1648ce825c31 628 pc.printf("STATUS: train 1 reached switch 3\n\r");
KoenKahlman 2:bb96eba66e78 629 std::time_t current = time(NULL);
KoenKahlman 2:bb96eba66e78 630 if(difftime(current, switch3action) < switchsafety){
KoenKahlman 2:bb96eba66e78 631 pc.printf("STATUS: Switch 3 on overheat cooldown!\n\r");
KoenKahlman 2:bb96eba66e78 632 return;
KoenKahlman 2:bb96eba66e78 633 }
KoenKahlman 1:1648ce825c31 634 stopTrain(1);
KoenKahlman 1:1648ce825c31 635 if(curr2 == 2 || next2 == 2){
KoenKahlman 1:1648ce825c31 636 //switch3 = true; TODO complicated
KoenKahlman 1:1648ce825c31 637 } else if(curr2 == 11 || next2 == 11){
KoenKahlman 1:1648ce825c31 638 //switch3 = false;
KoenKahlman 1:1648ce825c31 639 } else { //other train far away
KoenKahlman 1:1648ce825c31 640 switch3 = rand()%2;
KoenKahlman 1:1648ce825c31 641 }
KoenKahlman 1:1648ce825c31 642 changeSwitch3(switch3);
KoenKahlman 2:bb96eba66e78 643 switch3action = time(NULL);
KoenKahlman 1:1648ce825c31 644 } else { //train == 2
KoenKahlman 1:1648ce825c31 645 if(next2 == 7){ return; }
KoenKahlman 1:1648ce825c31 646 pc.printf("STATUS: train 2 reached switch 3\n\r");
KoenKahlman 2:bb96eba66e78 647 std::time_t current = time(NULL);
KoenKahlman 2:bb96eba66e78 648 if(difftime(current, switch3action) < switchsafety){
KoenKahlman 2:bb96eba66e78 649 pc.printf("STATUS: Switch 3 on overheat cooldown!\n\r");
KoenKahlman 2:bb96eba66e78 650 return;
KoenKahlman 2:bb96eba66e78 651 }
KoenKahlman 1:1648ce825c31 652 stopTrain(2);
KoenKahlman 1:1648ce825c31 653 if(curr1 == 2 || next1 == 2){
KoenKahlman 1:1648ce825c31 654 //switch3 = true;
KoenKahlman 1:1648ce825c31 655 } else if(curr1 == 11 || next1 == 11){
KoenKahlman 1:1648ce825c31 656 //switch3 = false;
KoenKahlman 1:1648ce825c31 657 } else { //other train far away
KoenKahlman 1:1648ce825c31 658 switch3 = rand()%2;
KoenKahlman 1:1648ce825c31 659 }
KoenKahlman 2:bb96eba66e78 660 changeSwitch3(switch3);
KoenKahlman 2:bb96eba66e78 661 switch3action = time(NULL);
KoenKahlman 1:1648ce825c31 662 }
KoenKahlman 1:1648ce825c31 663 }
KoenKahlman 1:1648ce825c31 664
KoenKahlman 1:1648ce825c31 665 void atSwitch4(int train){
KoenKahlman 1:1648ce825c31 666 if(train == 1){
KoenKahlman 1:1648ce825c31 667 if(next1 == 7){ return; }
KoenKahlman 1:1648ce825c31 668 pc.printf("STATUS: train 1 reached switch 4\n\r");
KoenKahlman 2:bb96eba66e78 669 std::time_t current = time(NULL);
KoenKahlman 2:bb96eba66e78 670 if(difftime(current, switch4action) < switchsafety){
KoenKahlman 2:bb96eba66e78 671 pc.printf("STATUS: Switch 4 on overheat cooldown!\n\r");
KoenKahlman 2:bb96eba66e78 672 return;
KoenKahlman 2:bb96eba66e78 673 }
KoenKahlman 1:1648ce825c31 674 stopTrain(1);
KoenKahlman 1:1648ce825c31 675 if(curr2 == 2 || next2 == 2){
KoenKahlman 1:1648ce825c31 676 //switch4 = true;
KoenKahlman 1:1648ce825c31 677 } else if(curr2 == 11 || next2 == 11){
KoenKahlman 1:1648ce825c31 678 //switch4 = false;
KoenKahlman 1:1648ce825c31 679 } else { //other train far away
KoenKahlman 1:1648ce825c31 680 switch4 = rand()%2;
KoenKahlman 1:1648ce825c31 681 }
KoenKahlman 1:1648ce825c31 682 changeSwitch4(switch4);
KoenKahlman 2:bb96eba66e78 683 switch4action = time(NULL);
KoenKahlman 1:1648ce825c31 684 } else { //train == 2
KoenKahlman 1:1648ce825c31 685 if(next2 == 7){ return; }
KoenKahlman 1:1648ce825c31 686 pc.printf("STATUS: train 2 reached switch 4\n\r");
KoenKahlman 2:bb96eba66e78 687 std::time_t current = time(NULL);
KoenKahlman 2:bb96eba66e78 688 if(difftime(current, switch4action) < switchsafety){
KoenKahlman 2:bb96eba66e78 689 pc.printf("STATUS: Switch 4 on overheat cooldown!\n\r");
KoenKahlman 2:bb96eba66e78 690 return;
KoenKahlman 2:bb96eba66e78 691 }
KoenKahlman 1:1648ce825c31 692 stopTrain(2);
KoenKahlman 1:1648ce825c31 693 if(curr1 == 2 || next1 == 2){
KoenKahlman 1:1648ce825c31 694 //switch4 = true;
KoenKahlman 1:1648ce825c31 695 } else if(curr1 == 11 || next1 == 11){
KoenKahlman 1:1648ce825c31 696 //switch4 = false;
KoenKahlman 1:1648ce825c31 697 } else { //other train far away
KoenKahlman 1:1648ce825c31 698 switch4 = rand()%2;
KoenKahlman 1:1648ce825c31 699 }
KoenKahlman 2:bb96eba66e78 700 changeSwitch4(switch4);
KoenKahlman 2:bb96eba66e78 701 switch4action = time(NULL);
KoenKahlman 1:1648ce825c31 702 }
KoenKahlman 1:1648ce825c31 703 }
KoenKahlman 1:1648ce825c31 704
KoenKahlman 1:1648ce825c31 705 void headCollision(void){
KoenKahlman 1:1648ce825c31 706 buzzer = 1;
KoenKahlman 1:1648ce825c31 707 pc.printf("WARNING: inbound head collision\n\r");
KoenKahlman 1:1648ce825c31 708 stopTrain(1);
KoenKahlman 1:1648ce825c31 709 stopTrain(2);
KoenKahlman 1:1648ce825c31 710 reverseTrain(1);
KoenKahlman 1:1648ce825c31 711 reverseTrain(2);
KoenKahlman 1:1648ce825c31 712 buzzer = 0;
KoenKahlman 1:1648ce825c31 713 }
KoenKahlman 1:1648ce825c31 714
KoenKahlman 1:1648ce825c31 715 void sideCollision(void){
KoenKahlman 1:1648ce825c31 716 buzzer = 1;
KoenKahlman 1:1648ce825c31 717 pc.printf("WARNING: possible side collision\n\r");
KoenKahlman 1:1648ce825c31 718 stopTrain(1);
KoenKahlman 1:1648ce825c31 719 stopTrain(2);
KoenKahlman 1:1648ce825c31 720 reverseTrain(1);
KoenKahlman 1:1648ce825c31 721 reverseTrain(2);
KoenKahlman 1:1648ce825c31 722 buzzer = 0;
KoenKahlman 1:1648ce825c31 723 }
KoenKahlman 1:1648ce825c31 724
KoenKahlman 1:1648ce825c31 725 void xCollision(void){
KoenKahlman 1:1648ce825c31 726 buzzer = 1;
KoenKahlman 1:1648ce825c31 727 pc.printf("WARNING: possible cross collision\n\r");
KoenKahlman 1:1648ce825c31 728 stopTrain(1);
KoenKahlman 1:1648ce825c31 729 stopTrain(2);
KoenKahlman 1:1648ce825c31 730 reverseTrain(1);
KoenKahlman 1:1648ce825c31 731 reverseTrain(2);
KoenKahlman 1:1648ce825c31 732 buzzer = 0;
KoenKahlman 1:1648ce825c31 733 }
KoenKahlman 1:1648ce825c31 734
KoenKahlman 1:1648ce825c31 735 void tailCollision(int backTrain){
KoenKahlman 1:1648ce825c31 736 buzzer = 1;
KoenKahlman 1:1648ce825c31 737 pc.printf("WARNING: possible tail collision\n\r");
KoenKahlman 1:1648ce825c31 738 stopTrain(backTrain);
KoenKahlman 1:1648ce825c31 739 reverseTrain(backTrain);
KoenKahlman 1:1648ce825c31 740 buzzer = 0;
KoenKahlman 1:1648ce825c31 741 }
KoenKahlman 1:1648ce825c31 742 /*
KoenKahlman 1:1648ce825c31 743 void loopDeltaCrossing(int train){
KoenKahlman 1:1648ce825c31 744 buzzer = 1;
KoenKahlman 1:1648ce825c31 745 pc.printf("WARNING: attempting to exit the loop\n\r");
KoenKahlman 1:1648ce825c31 746 if(train == 1){
KoenKahlman 1:1648ce825c31 747 if(curr2 != 13 && curr2 != 0 && curr2 != 1 && curr2 != 2){
KoenKahlman 1:1648ce825c31 748 stopTrain(1);
KoenKahlman 1:1648ce825c31 749 reverseTrain(1);
KoenKahlman 1:1648ce825c31 750 }
KoenKahlman 1:1648ce825c31 751 //if train 2 in loop
KoenKahlman 1:1648ce825c31 752 //stop, reverse
KoenKahlman 1:1648ce825c31 753 } else { //train == 2
KoenKahlman 1:1648ce825c31 754 //if train 1 in loop
KoenKahlman 1:1648ce825c31 755 //stop, reverse
KoenKahlman 1:1648ce825c31 756 if(curr1 != 13 && curr1 != 0 && curr1 != 1 && curr1 != 2){
KoenKahlman 1:1648ce825c31 757 stopTrain(1);
KoenKahlman 1:1648ce825c31 758 reverseTrain(1);
KoenKahlman 1:1648ce825c31 759 }
KoenKahlman 1:1648ce825c31 760 }
KoenKahlman 1:1648ce825c31 761 buzzer = 0;
KoenKahlman 1:1648ce825c31 762 }
KoenKahlman 1:1648ce825c31 763 */
KoenKahlman 0:9c82986d7cb9 764
KoenKahlman 0:9c82986d7cb9 765
KoenKahlman 0:9c82986d7cb9 766
KoenKahlman 0:9c82986d7cb9 767
KoenKahlman 0:9c82986d7cb9 768 //TRACK COMMANDS
KoenKahlman 0:9c82986d7cb9 769 //code from the tutorial, sends a message to the track through the train pin
KoenKahlman 0:9c82986d7cb9 770 void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count){
KoenKahlman 0:9c82986d7cb9 771 switch(address){
KoenKahlman 1:1648ce825c31 772 case DCCaddress_train1: led1 = 1; break;
KoenKahlman 1:1648ce825c31 773 case DCCaddress_train2: led2 = 1; break;
KoenKahlman 1:1648ce825c31 774 case DCCaddress_decoder: led3 = 1; break;
KoenKahlman 0:9c82986d7cb9 775 }
KoenKahlman 0:9c82986d7cb9 776
KoenKahlman 0:9c82986d7cb9 777 unsigned __int64 command = 0x0000000000000000; // __int64 is the 64-bit integer type
KoenKahlman 0:9c82986d7cb9 778 unsigned __int64 temp_command = 0x0000000000000000;
KoenKahlman 0:9c82986d7cb9 779 unsigned __int64 prefix = 0x3FFF; // 14 "1" bits needed at start
KoenKahlman 0:9c82986d7cb9 780 unsigned int error = 0x00; //error byte
KoenKahlman 0:9c82986d7cb9 781 //calculate error detection byte with xor
KoenKahlman 0:9c82986d7cb9 782 error = address ^ inst;
KoenKahlman 0:9c82986d7cb9 783 //combine packet bits in basic DCC format
KoenKahlman 0:9c82986d7cb9 784 command = (prefix<<28)|(address<<19)|(inst<<10)|((error)<<1)|0x01;
KoenKahlman 0:9c82986d7cb9 785 //printf("\n\r %llx \n\r",command);
KoenKahlman 0:9c82986d7cb9 786 int i=0;
KoenKahlman 0:9c82986d7cb9 787 //repeat DCC command lots of times
KoenKahlman 0:9c82986d7cb9 788 while(i < repeat_count) {
KoenKahlman 0:9c82986d7cb9 789 temp_command = command;
KoenKahlman 0:9c82986d7cb9 790 //loops through packet bits encoding and sending out digital pulses for a DCC command
KoenKahlman 0:9c82986d7cb9 791 for (int j=0; j<64; j++) {
KoenKahlman 0:9c82986d7cb9 792 if((temp_command&0x8000000000000000)==0) { //test packet bit
KoenKahlman 0:9c82986d7cb9 793 //send data for a "0" bit
KoenKahlman 0:9c82986d7cb9 794 data=0;
KoenKahlman 0:9c82986d7cb9 795 wait_us(100);
KoenKahlman 0:9c82986d7cb9 796 data=1;
KoenKahlman 0:9c82986d7cb9 797 wait_us(100);
KoenKahlman 0:9c82986d7cb9 798 //printf("0011");
KoenKahlman 0:9c82986d7cb9 799 } else {
KoenKahlman 0:9c82986d7cb9 800 //send data for a "1"bit
KoenKahlman 0:9c82986d7cb9 801 data=0;
KoenKahlman 0:9c82986d7cb9 802 wait_us(58);
KoenKahlman 0:9c82986d7cb9 803 data=1;
KoenKahlman 0:9c82986d7cb9 804 wait_us(58);
KoenKahlman 0:9c82986d7cb9 805 //printf("01");
KoenKahlman 0:9c82986d7cb9 806 }
KoenKahlman 0:9c82986d7cb9 807 // next bit in packet
KoenKahlman 0:9c82986d7cb9 808 temp_command = temp_command<<1;
KoenKahlman 0:9c82986d7cb9 809 }
KoenKahlman 0:9c82986d7cb9 810 i++;
KoenKahlman 0:9c82986d7cb9 811 }
KoenKahlman 0:9c82986d7cb9 812
KoenKahlman 0:9c82986d7cb9 813 led1 = 0;
KoenKahlman 0:9c82986d7cb9 814 led2 = 0;
KoenKahlman 1:1648ce825c31 815 led3 = 0;
KoenKahlman 0:9c82986d7cb9 816 }
KoenKahlman 0:9c82986d7cb9 817
KoenKahlman 0:9c82986d7cb9 818 void changeSwitch1(bool inwards){
KoenKahlman 0:9c82986d7cb9 819 if(inwards){
KoenKahlman 0:9c82986d7cb9 820 DCC_send_command(DCCaddress_decoder, DCCswitch1, 1);
KoenKahlman 1:1648ce825c31 821 pc.printf("STATUS: Switch 1 changed inward\n\r");
KoenKahlman 0:9c82986d7cb9 822 } else {
KoenKahlman 0:9c82986d7cb9 823 DCC_send_command(DCCaddress_decoder, DCCswitch1, 1);
KoenKahlman 0:9c82986d7cb9 824 DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
KoenKahlman 1:1648ce825c31 825 pc.printf("STATUS: Switch 1 changed outward\n\r");
KoenKahlman 0:9c82986d7cb9 826 }
KoenKahlman 1:1648ce825c31 827 updateTrainPredictions();
KoenKahlman 0:9c82986d7cb9 828 }
KoenKahlman 0:9c82986d7cb9 829
KoenKahlman 0:9c82986d7cb9 830 void changeSwitch2(bool inwards){
KoenKahlman 0:9c82986d7cb9 831 if(inwards){
KoenKahlman 0:9c82986d7cb9 832 DCC_send_command(DCCaddress_decoder, DCCswitch2, 1);
KoenKahlman 1:1648ce825c31 833 pc.printf("STATUS: Switch 2 changed inward\n\r");
KoenKahlman 0:9c82986d7cb9 834 } else {
KoenKahlman 0:9c82986d7cb9 835 DCC_send_command(DCCaddress_decoder, DCCswitch2, 1);
KoenKahlman 0:9c82986d7cb9 836 DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
KoenKahlman 1:1648ce825c31 837 pc.printf("STATUS: Switch 2 changed outward\n\r");
KoenKahlman 1:1648ce825c31 838 }
KoenKahlman 1:1648ce825c31 839 updateTrainPredictions();
KoenKahlman 0:9c82986d7cb9 840 }
KoenKahlman 0:9c82986d7cb9 841
KoenKahlman 0:9c82986d7cb9 842 void changeSwitch3(bool inwards){
KoenKahlman 0:9c82986d7cb9 843 if(inwards){
KoenKahlman 0:9c82986d7cb9 844 DCC_send_command(DCCaddress_decoder, DCCswitch3, 1);
KoenKahlman 0:9c82986d7cb9 845 DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1); //switch 3 has reverse behavior
KoenKahlman 1:1648ce825c31 846 pc.printf("STATUS: Switch 3 changed inward\n\r");
KoenKahlman 0:9c82986d7cb9 847 } else {
KoenKahlman 0:9c82986d7cb9 848 DCC_send_command(DCCaddress_decoder, DCCswitch3, 1);
KoenKahlman 1:1648ce825c31 849 pc.printf("STATUS: Switch 3 changed outward\n\r");
KoenKahlman 1:1648ce825c31 850 }
KoenKahlman 1:1648ce825c31 851 updateTrainPredictions();
KoenKahlman 0:9c82986d7cb9 852 }
KoenKahlman 0:9c82986d7cb9 853
KoenKahlman 0:9c82986d7cb9 854 void changeSwitch4(bool inwards){
KoenKahlman 0:9c82986d7cb9 855 if(inwards){
KoenKahlman 0:9c82986d7cb9 856 DCC_send_command(DCCaddress_decoder, DCCswitch4, 1);
KoenKahlman 1:1648ce825c31 857 pc.printf("STATUS: Switch 4 changed inward\n\r");
KoenKahlman 0:9c82986d7cb9 858 } else {
KoenKahlman 0:9c82986d7cb9 859 DCC_send_command(DCCaddress_decoder, DCCswitch4, 1);
KoenKahlman 0:9c82986d7cb9 860 DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
KoenKahlman 1:1648ce825c31 861 pc.printf("STATUS: Switch 4 changed outward\n\r");
KoenKahlman 1:1648ce825c31 862 }
KoenKahlman 1:1648ce825c31 863 updateTrainPredictions();
KoenKahlman 1:1648ce825c31 864 }
KoenKahlman 1:1648ce825c31 865
KoenKahlman 1:1648ce825c31 866 void updateTrainPredictions(){
KoenKahlman 1:1648ce825c31 867 next1 = predictNextMarker(prev1, curr1);
KoenKahlman 1:1648ce825c31 868 next2 = predictNextMarker(prev2, curr2);
KoenKahlman 0:9c82986d7cb9 869 }
KoenKahlman 0:9c82986d7cb9 870
KoenKahlman 0:9c82986d7cb9 871 unsigned int trainInstruction(bool direction, bool light, int speed){
KoenKahlman 0:9c82986d7cb9 872 unsigned int result = 0x40; //0100 0000
KoenKahlman 0:9c82986d7cb9 873 if(direction){
KoenKahlman 0:9c82986d7cb9 874 result += 0x20; //0010 0000
KoenKahlman 0:9c82986d7cb9 875 }
KoenKahlman 0:9c82986d7cb9 876 if(light){
KoenKahlman 0:9c82986d7cb9 877 result += 0x10; //0001 0000
KoenKahlman 0:9c82986d7cb9 878 }
KoenKahlman 0:9c82986d7cb9 879 if(speed>0 && speed<15){
KoenKahlman 0:9c82986d7cb9 880 result += speed + 1;
KoenKahlman 0:9c82986d7cb9 881 }
KoenKahlman 0:9c82986d7cb9 882 return result;
KoenKahlman 0:9c82986d7cb9 883 }
KoenKahlman 0:9c82986d7cb9 884
KoenKahlman 0:9c82986d7cb9 885 //INTERPRET INTERRUPT SIGNALS
KoenKahlman 0:9c82986d7cb9 886 //INTERRUPTS
KoenKahlman 0:9c82986d7cb9 887 //code from the tutorial, initializes MCP and sets it up for interrupts
KoenKahlman 0:9c82986d7cb9 888 void init_mcp() {
KoenKahlman 0:9c82986d7cb9 889 // Initialisation of MCP registers, documentation on registers is available at
KoenKahlman 0:9c82986d7cb9 890 //Niels/Abel/Robert/Natalia
KoenKahlman 0:9c82986d7cb9 891 mcp = new MCP23017(i2c, 0x40);
KoenKahlman 0:9c82986d7cb9 892 mcp->_write(IODIRA, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 893 mcp->_write(IODIRB, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 894 mcp->_write(IPOLA, (unsigned char )0x00);
KoenKahlman 0:9c82986d7cb9 895 mcp->_write(IPOLB, (unsigned char )0x00);
KoenKahlman 0:9c82986d7cb9 896 mcp->_write(DEFVALA, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 897 mcp->_write(DEFVALB, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 898 mcp->_write(INTCONA, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 899 mcp->_write(INTCONB, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 900 mcp->_write(IOCONA, (unsigned char )0x2);
KoenKahlman 0:9c82986d7cb9 901 mcp->_write(IOCONB, (unsigned char )0x2);
KoenKahlman 0:9c82986d7cb9 902 mcp->_write(GPPUA, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 903 mcp->_write(GPPUB, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 904
KoenKahlman 0:9c82986d7cb9 905 // Clear current interrupts
KoenKahlman 0:9c82986d7cb9 906 mcp->_read(GPIOA);
KoenKahlman 0:9c82986d7cb9 907 mcp->_read(GPIOB);
KoenKahlman 0:9c82986d7cb9 908 // Register callbacks
KoenKahlman 0:9c82986d7cb9 909 int0.fall(&on_int0_change);
KoenKahlman 0:9c82986d7cb9 910 int1.fall(&on_int1_change);
KoenKahlman 0:9c82986d7cb9 911 // Enable interrupts on the MCP
KoenKahlman 0:9c82986d7cb9 912 mcp->_write(GPINTENA, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 913 mcp->_write(GPINTENB, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 914
KoenKahlman 0:9c82986d7cb9 915 int0flag = false;
KoenKahlman 0:9c82986d7cb9 916 int1flag = false;
KoenKahlman 0:9c82986d7cb9 917 }
KoenKahlman 0:9c82986d7cb9 918
KoenKahlman 0:9c82986d7cb9 919 void on_int0_change() {
KoenKahlman 0:9c82986d7cb9 920 int0flag = true;
KoenKahlman 0:9c82986d7cb9 921 }
KoenKahlman 0:9c82986d7cb9 922
KoenKahlman 0:9c82986d7cb9 923 void on_int1_change() {
KoenKahlman 0:9c82986d7cb9 924 int1flag = true;
KoenKahlman 0:9c82986d7cb9 925 }
KoenKahlman 0:9c82986d7cb9 926
KoenKahlman 0:9c82986d7cb9 927 std::list<int> circuit0markers(int sensor_data){
KoenKahlman 0:9c82986d7cb9 928 std::list<int> result;
KoenKahlman 0:9c82986d7cb9 929 sensor_data = ~sensor_data;
KoenKahlman 0:9c82986d7cb9 930 if((sensor_data & 0b00000001) == 0b00000001){
KoenKahlman 0:9c82986d7cb9 931 result.push_back(0);
KoenKahlman 0:9c82986d7cb9 932 }
KoenKahlman 0:9c82986d7cb9 933 if((sensor_data & 0b00000010) == 0b00000010){
KoenKahlman 0:9c82986d7cb9 934 result.push_back(1);
KoenKahlman 0:9c82986d7cb9 935 }
KoenKahlman 0:9c82986d7cb9 936 if((sensor_data & 0b00000100) == 0b00000100){
KoenKahlman 0:9c82986d7cb9 937 result.push_back(2);
KoenKahlman 0:9c82986d7cb9 938 }
KoenKahlman 0:9c82986d7cb9 939 if((sensor_data & 0b00001000) == 0b00001000){
KoenKahlman 0:9c82986d7cb9 940 result.push_back(3);
KoenKahlman 0:9c82986d7cb9 941 }
KoenKahlman 0:9c82986d7cb9 942 if((sensor_data & 0b00010000) == 0b00010000){
KoenKahlman 0:9c82986d7cb9 943 result.push_back(4);
KoenKahlman 0:9c82986d7cb9 944 }
KoenKahlman 0:9c82986d7cb9 945 if((sensor_data & 0b00100000) == 0b00100000){
KoenKahlman 0:9c82986d7cb9 946 result.push_back(5);
KoenKahlman 0:9c82986d7cb9 947 }
KoenKahlman 0:9c82986d7cb9 948 if((sensor_data & 0b01000000) == 0b01000000){
KoenKahlman 0:9c82986d7cb9 949 result.push_back(6);
KoenKahlman 0:9c82986d7cb9 950 }
KoenKahlman 0:9c82986d7cb9 951 if((sensor_data & 0b10000000) == 0b10000000){
KoenKahlman 0:9c82986d7cb9 952 result.push_back(7);
KoenKahlman 0:9c82986d7cb9 953 }
KoenKahlman 0:9c82986d7cb9 954 return result;
KoenKahlman 0:9c82986d7cb9 955 }
KoenKahlman 0:9c82986d7cb9 956
KoenKahlman 0:9c82986d7cb9 957 std::list<int> circuit1markers(int sensor_data){
KoenKahlman 0:9c82986d7cb9 958 std::list<int> result;
KoenKahlman 0:9c82986d7cb9 959 sensor_data = ~sensor_data;
KoenKahlman 0:9c82986d7cb9 960 if((sensor_data & 0b00000001) == 0b00000001){
KoenKahlman 0:9c82986d7cb9 961 result.push_back(8);
KoenKahlman 0:9c82986d7cb9 962 }
KoenKahlman 0:9c82986d7cb9 963 if((sensor_data & 0b00000010) == 0b00000010){
KoenKahlman 0:9c82986d7cb9 964 result.push_back(9);
KoenKahlman 0:9c82986d7cb9 965 }
KoenKahlman 0:9c82986d7cb9 966 if((sensor_data & 0b00000100) == 0b00000100){
KoenKahlman 0:9c82986d7cb9 967 result.push_back(10);
KoenKahlman 0:9c82986d7cb9 968 }
KoenKahlman 0:9c82986d7cb9 969 if((sensor_data & 0b00001000) == 0b00001000){
KoenKahlman 0:9c82986d7cb9 970 result.push_back(11);
KoenKahlman 0:9c82986d7cb9 971 }
KoenKahlman 0:9c82986d7cb9 972 if((sensor_data & 0b00010000) == 0b00010000){
KoenKahlman 0:9c82986d7cb9 973 result.push_back(12);
KoenKahlman 0:9c82986d7cb9 974 }
KoenKahlman 0:9c82986d7cb9 975 if((sensor_data & 0b00100000) == 0b00100000){
KoenKahlman 0:9c82986d7cb9 976 result.push_back(13);
KoenKahlman 0:9c82986d7cb9 977 }
KoenKahlman 0:9c82986d7cb9 978 if((sensor_data & 0b01000000) == 0b01000000){
KoenKahlman 1:1648ce825c31 979 //result.push_back(21); do nothing; sensor 21 badly positioned
KoenKahlman 0:9c82986d7cb9 980 }
KoenKahlman 0:9c82986d7cb9 981 if((sensor_data & 0b10000000) == 0b10000000){
KoenKahlman 1:1648ce825c31 982 //result.push_back(22); do nothing; too close
KoenKahlman 0:9c82986d7cb9 983 }
KoenKahlman 0:9c82986d7cb9 984 return result;
KoenKahlman 0:9c82986d7cb9 985 }
KoenKahlman 0:9c82986d7cb9 986
KoenKahlman 0:9c82986d7cb9 987 //LOG OUTPUT
KoenKahlman 0:9c82986d7cb9 988 void displayLCDstatus(){
KoenKahlman 0:9c82986d7cb9 989 lcd.cls();
KoenKahlman 1:1648ce825c31 990 lcd.printf("Train 1: %d \nTrain 2: %d", speed1, speed2);
KoenKahlman 0:9c82986d7cb9 991 }
KoenKahlman 0:9c82986d7cb9 992
KoenKahlman 0:9c82986d7cb9 993 void displayTerminalStatus(){
KoenKahlman 0:9c82986d7cb9 994 string dir1Text = dir1?"forward":"reverse";
KoenKahlman 0:9c82986d7cb9 995 string dir2Text = dir2?"forward":"reverse";
KoenKahlman 0:9c82986d7cb9 996 string light1Text = light1?"on, ":"off,";
KoenKahlman 0:9c82986d7cb9 997 string light2Text = light2?"on, ":"off,";
KoenKahlman 1:1648ce825c31 998 pc.printf("SYSTEM: Iteration %d\n\r", iter);
KoenKahlman 1:1648ce825c31 999 pc.printf("Train 1: dir %s, light %s spd %d\n\rTrain 2: dir %s, light %s spd %d\n\r", dir1Text.c_str(), light1Text.c_str(), speed1, dir2Text.c_str(), light2Text.c_str(), speed2);
KoenKahlman 0:9c82986d7cb9 1000 }