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@1:1648ce825c31, 2019-06-27 (annotated)

Committer:
KoenKahlman
Date:
Thu Jun 27 07:50:27 2019 +0000
Revision:
1:1648ce825c31
Parent:
0:9c82986d7cb9
Child:
2:bb96eba66e78
version for testing; ; unimplemented: deltaloop safety, accounting for power surges, switch safety timers

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 0:9c82986d7cb9 146
KoenKahlman 0:9c82986d7cb9 147 if(!test){
KoenKahlman 0:9c82986d7cb9 148 iter = 0; //iter count
KoenKahlman 0:9c82986d7cb9 149 while(enable){
KoenKahlman 0:9c82986d7cb9 150 if(!button4){
KoenKahlman 0:9c82986d7cb9 151 emergencyStop();
KoenKahlman 0:9c82986d7cb9 152 break;
KoenKahlman 1:1648ce825c31 153 }
KoenKahlman 1:1648ce825c31 154
KoenKahlman 0:9c82986d7cb9 155 if(int0flag){
KoenKahlman 0:9c82986d7cb9 156 handleInt0(); //store passed circuit 0 markers in the list passed markers
KoenKahlman 0:9c82986d7cb9 157 }
KoenKahlman 0:9c82986d7cb9 158
KoenKahlman 0:9c82986d7cb9 159 if(int1flag){
KoenKahlman 0:9c82986d7cb9 160 handleInt1(); //same for circuit 1
KoenKahlman 0:9c82986d7cb9 161 }
KoenKahlman 0:9c82986d7cb9 162
KoenKahlman 1:1648ce825c31 163 if(!passedMarkers.empty()){
KoenKahlman 0:9c82986d7cb9 164 for (it=passedMarkers.begin(); it!=passedMarkers.end(); ++it){
KoenKahlman 1:1648ce825c31 165 if(*it != next1 && *it != next2 && *it != curr1 && *it != curr2){ //TODO check if safe
KoenKahlman 1:1648ce825c31 166 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 167 enable = false;
KoenKahlman 1:1648ce825c31 168 break;
KoenKahlman 1:1648ce825c31 169 }
KoenKahlman 0:9c82986d7cb9 170 updateTrainStatus(*it);
KoenKahlman 0:9c82986d7cb9 171 }
KoenKahlman 0:9c82986d7cb9 172 passedMarkers.clear();
KoenKahlman 0:9c82986d7cb9 173 }
KoenKahlman 0:9c82986d7cb9 174
KoenKahlman 0:9c82986d7cb9 175 trainCommand(); //decides for a command to send to the track
KoenKahlman 0:9c82986d7cb9 176
KoenKahlman 1:1648ce825c31 177 iterAction(); //every X iterations display some stuff
KoenKahlman 0:9c82986d7cb9 178 iter++;
KoenKahlman 0:9c82986d7cb9 179 }
KoenKahlman 0:9c82986d7cb9 180 } else {
KoenKahlman 0:9c82986d7cb9 181 //test scripts
KoenKahlman 0:9c82986d7cb9 182 }
KoenKahlman 0:9c82986d7cb9 183 }
KoenKahlman 0:9c82986d7cb9 184 }
KoenKahlman 0:9c82986d7cb9 185
KoenKahlman 0:9c82986d7cb9 186 //MAIN LOOP ROUTINES
KoenKahlman 1:1648ce825c31 187 void init(){
KoenKahlman 1:1648ce825c31 188 command1 = false;
KoenKahlman 1:1648ce825c31 189 command2 = false;
KoenKahlman 1:1648ce825c31 190 collision = false;
KoenKahlman 1:1648ce825c31 191
KoenKahlman 0:9c82986d7cb9 192 //initialize clock time
KoenKahlman 0:9c82986d7cb9 193 set_time(1);
KoenKahlman 0:9c82986d7cb9 194 switch1action = time(NULL);
KoenKahlman 0:9c82986d7cb9 195 switch2action = time(NULL);
KoenKahlman 0:9c82986d7cb9 196 switch3action = time(NULL);
KoenKahlman 0:9c82986d7cb9 197 switch4action = time(NULL);
KoenKahlman 0:9c82986d7cb9 198
KoenKahlman 0:9c82986d7cb9 199 led1 = 0;
KoenKahlman 0:9c82986d7cb9 200 led2 = 0;
KoenKahlman 0:9c82986d7cb9 201 led3 = 0;
KoenKahlman 1:1648ce825c31 202 buzzer = 0;
KoenKahlman 1:1648ce825c31 203 boxled1 = 0;
KoenKahlman 1:1648ce825c31 204 boxled2 = 0;
KoenKahlman 1:1648ce825c31 205
KoenKahlman 1:1648ce825c31 206 speedsetting = 7;
KoenKahlman 0:9c82986d7cb9 207
KoenKahlman 0:9c82986d7cb9 208 //pc.baud(921600); //faster printing
KoenKahlman 0:9c82986d7cb9 209 pc.baud(19200);
KoenKahlman 0:9c82986d7cb9 210
KoenKahlman 0:9c82986d7cb9 211 //initialize mcp //(initialize interrupt flags)
KoenKahlman 0:9c82986d7cb9 212 init_mcp();
KoenKahlman 0:9c82986d7cb9 213
KoenKahlman 0:9c82986d7cb9 214 //initialize train globals
KoenKahlman 0:9c82986d7cb9 215 dir1 = true;
KoenKahlman 0:9c82986d7cb9 216 light1 = true;
KoenKahlman 1:1648ce825c31 217 speed1 = speedsetting;
KoenKahlman 1:1648ce825c31 218 boarding1 = false;
KoenKahlman 1:1648ce825c31 219 //train 1 starting position
KoenKahlman 1:1648ce825c31 220 prev1 = 0;
KoenKahlman 1:1648ce825c31 221 curr1 = 1;
KoenKahlman 1:1648ce825c31 222 next1 = predictNextMarker(prev1, curr1); //2
KoenKahlman 0:9c82986d7cb9 223
KoenKahlman 0:9c82986d7cb9 224 //global variables for train2
KoenKahlman 1:1648ce825c31 225 dir2 = true;
KoenKahlman 0:9c82986d7cb9 226 light2 = true;
KoenKahlman 1:1648ce825c31 227 speed2 = speedsetting;
KoenKahlman 1:1648ce825c31 228 boarding2 = false;
KoenKahlman 1:1648ce825c31 229 //train 2 starting position
KoenKahlman 1:1648ce825c31 230 prev2 = 4;
KoenKahlman 1:1648ce825c31 231 curr2 = 6;
KoenKahlman 1:1648ce825c31 232 next2 = predictNextMarker(prev2, curr2); //7
KoenKahlman 0:9c82986d7cb9 233
KoenKahlman 1:1648ce825c31 234 //initialize presumed switch positions
KoenKahlman 0:9c82986d7cb9 235 switch1 = false;
KoenKahlman 0:9c82986d7cb9 236 switch2 = false;
KoenKahlman 0:9c82986d7cb9 237 switch3 = false;
KoenKahlman 0:9c82986d7cb9 238 switch4 = false;
KoenKahlman 1:1648ce825c31 239 }
KoenKahlman 1:1648ce825c31 240
KoenKahlman 1:1648ce825c31 241 void startupWait(){
KoenKahlman 1:1648ce825c31 242 lcd.cls();
KoenKahlman 1:1648ce825c31 243 lcd.printf("Press START to start");
KoenKahlman 1:1648ce825c31 244 pc.printf("Press START to start\n\r");
KoenKahlman 1:1648ce825c31 245 while(button3){
KoenKahlman 1:1648ce825c31 246 //do nothing
KoenKahlman 1:1648ce825c31 247 }
KoenKahlman 1:1648ce825c31 248 pc.printf("Starting...\n\r");
KoenKahlman 0:9c82986d7cb9 249 }
KoenKahlman 0:9c82986d7cb9 250
KoenKahlman 0:9c82986d7cb9 251 void initSwitches(){
KoenKahlman 0:9c82986d7cb9 252 wait_ms(switchwait);
KoenKahlman 1:1648ce825c31 253 changeSwitch1(switch1);
KoenKahlman 0:9c82986d7cb9 254 wait_ms(switchwait);
KoenKahlman 1:1648ce825c31 255 changeSwitch2(switch2);
KoenKahlman 0:9c82986d7cb9 256 wait_ms(switchwait);
KoenKahlman 1:1648ce825c31 257 changeSwitch3(switch3);
KoenKahlman 0:9c82986d7cb9 258 wait_ms(switchwait);
KoenKahlman 1:1648ce825c31 259 changeSwitch4(switch4);
KoenKahlman 1:1648ce825c31 260 wait_ms(500);
KoenKahlman 0:9c82986d7cb9 261 }
KoenKahlman 0:9c82986d7cb9 262
KoenKahlman 1:1648ce825c31 263 void emergencyStop(){
KoenKahlman 1:1648ce825c31 264 enable = false;
KoenKahlman 1:1648ce825c31 265 pc.printf("Emergency stop! Press button 3 when ready to restart\n\r");
KoenKahlman 1:1648ce825c31 266
KoenKahlman 1:1648ce825c31 267 lcd.cls();
KoenKahlman 1:1648ce825c31 268 lcd.printf("Press START when ready to restart");
KoenKahlman 1:1648ce825c31 269 while(button3){
KoenKahlman 1:1648ce825c31 270 //do nothing
KoenKahlman 0:9c82986d7cb9 271 }
KoenKahlman 0:9c82986d7cb9 272 }
KoenKahlman 0:9c82986d7cb9 273
KoenKahlman 0:9c82986d7cb9 274 void handleInt0(){
KoenKahlman 0:9c82986d7cb9 275 wait_us(2000);
KoenKahlman 0:9c82986d7cb9 276 int sensor_data = mcp->_read(INTCAPA);
KoenKahlman 0:9c82986d7cb9 277 int0flag = false;
KoenKahlman 0:9c82986d7cb9 278 std::list<int> temp = circuit0markers(sensor_data);
KoenKahlman 0:9c82986d7cb9 279 passedMarkers.splice(passedMarkers.end(), temp);
KoenKahlman 0:9c82986d7cb9 280 }
KoenKahlman 0:9c82986d7cb9 281
KoenKahlman 0:9c82986d7cb9 282 void handleInt1(){
KoenKahlman 0:9c82986d7cb9 283 wait_us(2000);
KoenKahlman 0:9c82986d7cb9 284 int sensor_data = mcp->_read(INTCAPB);
KoenKahlman 0:9c82986d7cb9 285 int1flag = false;
KoenKahlman 0:9c82986d7cb9 286 std::list<int> temp = circuit1markers(sensor_data);
KoenKahlman 0:9c82986d7cb9 287 passedMarkers.splice(passedMarkers.end(), temp);
KoenKahlman 0:9c82986d7cb9 288 }
KoenKahlman 0:9c82986d7cb9 289
KoenKahlman 1:1648ce825c31 290 void updateTrainStatus(int marker){
KoenKahlman 1:1648ce825c31 291 if(marker == 21){
KoenKahlman 1:1648ce825c31 292 return; //marker 21 is unused
KoenKahlman 1:1648ce825c31 293 }
KoenKahlman 1:1648ce825c31 294
KoenKahlman 1:1648ce825c31 295 if(marker == next1){
KoenKahlman 1:1648ce825c31 296 prev1 = curr1;
KoenKahlman 1:1648ce825c31 297 curr1 = next1;
KoenKahlman 1:1648ce825c31 298 next1 = predictNextMarker(prev1, curr1);
KoenKahlman 1:1648ce825c31 299 command1 = false;
KoenKahlman 1:1648ce825c31 300 collision = false;
KoenKahlman 1:1648ce825c31 301 pc.printf("STATUS: Train 1 reached marker %d from marker %d. Predicted marker %d\n\r", curr1, prev1, next1);
KoenKahlman 1:1648ce825c31 302 } else { //marker == next2
KoenKahlman 1:1648ce825c31 303 prev2 = curr2;
KoenKahlman 1:1648ce825c31 304 curr2 = next2;
KoenKahlman 1:1648ce825c31 305 next2 = predictNextMarker(prev2, curr2);
KoenKahlman 1:1648ce825c31 306 command2 = false;
KoenKahlman 1:1648ce825c31 307 collision = false;
KoenKahlman 1:1648ce825c31 308 pc.printf("STATUS: Train 2 reached marker %d from marker %d. Predicted marker %d\n\r", curr2, prev2, next2);
KoenKahlman 1:1648ce825c31 309 }
KoenKahlman 1:1648ce825c31 310 }
KoenKahlman 1:1648ce825c31 311
KoenKahlman 1:1648ce825c31 312 int predictNextMarker(int prev, int curr){
KoenKahlman 1:1648ce825c31 313 switch(prev){
KoenKahlman 1:1648ce825c31 314 case 0: switch(curr){
KoenKahlman 1:1648ce825c31 315 case 1: return 2;
KoenKahlman 1:1648ce825c31 316 case 13: return 12;
KoenKahlman 1:1648ce825c31 317 }
KoenKahlman 1:1648ce825c31 318 case 1: switch(curr){
KoenKahlman 1:1648ce825c31 319 case 2: return switch2?3:4;
KoenKahlman 1:1648ce825c31 320 case 0: return 13;
KoenKahlman 1:1648ce825c31 321 }
KoenKahlman 1:1648ce825c31 322 case 2: switch(curr){
KoenKahlman 1:1648ce825c31 323 case 3: return 9;
KoenKahlman 1:1648ce825c31 324 case 4: return 6;
KoenKahlman 1:1648ce825c31 325 case 1: return 0;
KoenKahlman 1:1648ce825c31 326 }
KoenKahlman 1:1648ce825c31 327 case 3: switch(curr){
KoenKahlman 1:1648ce825c31 328 case 9: return 8;
KoenKahlman 1:1648ce825c31 329 case 2: return 1;
KoenKahlman 1:1648ce825c31 330 }
KoenKahlman 1:1648ce825c31 331 case 4: switch(curr){
KoenKahlman 1:1648ce825c31 332 case 6: return 7;
KoenKahlman 1:1648ce825c31 333 case 2: return 1;
KoenKahlman 1:1648ce825c31 334 }
KoenKahlman 1:1648ce825c31 335 case 5: switch(curr){
KoenKahlman 1:1648ce825c31 336 case 6: return 7;
KoenKahlman 1:1648ce825c31 337 case 11: return 12;
KoenKahlman 1:1648ce825c31 338 }
KoenKahlman 1:1648ce825c31 339 case 6: switch(curr){
KoenKahlman 1:1648ce825c31 340 case 4: return 2;
KoenKahlman 1:1648ce825c31 341 case 5: return 11;
KoenKahlman 1:1648ce825c31 342 case 7: return 8;
KoenKahlman 1:1648ce825c31 343 }
KoenKahlman 1:1648ce825c31 344 case 7: switch(curr){
KoenKahlman 1:1648ce825c31 345 case 6: return switch3?5:4;
KoenKahlman 1:1648ce825c31 346 case 8: return switch4?9:10;
KoenKahlman 1:1648ce825c31 347 }
KoenKahlman 1:1648ce825c31 348 case 8: switch(curr){
KoenKahlman 1:1648ce825c31 349 case 7: return 6;
KoenKahlman 1:1648ce825c31 350 case 9: return 3;
KoenKahlman 1:1648ce825c31 351 case 10: return 12;
KoenKahlman 1:1648ce825c31 352 }
KoenKahlman 1:1648ce825c31 353 case 9: switch(curr){
KoenKahlman 1:1648ce825c31 354 case 3: return 2;
KoenKahlman 1:1648ce825c31 355 case 8: return 7;
KoenKahlman 1:1648ce825c31 356 }
KoenKahlman 1:1648ce825c31 357 case 10: switch(curr){
KoenKahlman 1:1648ce825c31 358 case 8: return 7;
KoenKahlman 1:1648ce825c31 359 case 12: return 13;
KoenKahlman 1:1648ce825c31 360 }
KoenKahlman 1:1648ce825c31 361 case 11: switch(curr){
KoenKahlman 1:1648ce825c31 362 case 5: return 6;
KoenKahlman 1:1648ce825c31 363 case 12: return 13;
KoenKahlman 1:1648ce825c31 364 }
KoenKahlman 1:1648ce825c31 365 case 12: switch(curr){
KoenKahlman 1:1648ce825c31 366 case 10: return 8;
KoenKahlman 1:1648ce825c31 367 case 11: return 5;
KoenKahlman 1:1648ce825c31 368 case 13: return 0;
KoenKahlman 1:1648ce825c31 369 }
KoenKahlman 1:1648ce825c31 370 case 13: switch(curr){
KoenKahlman 1:1648ce825c31 371 case 0: return 1;
KoenKahlman 1:1648ce825c31 372 case 12: return switch1?11:10;
KoenKahlman 1:1648ce825c31 373 }
KoenKahlman 1:1648ce825c31 374 }
KoenKahlman 1:1648ce825c31 375 return 666; //TODO default
KoenKahlman 1:1648ce825c31 376 }
KoenKahlman 1:1648ce825c31 377
KoenKahlman 1:1648ce825c31 378 void iterAction(){
KoenKahlman 1:1648ce825c31 379 displayLCDstatus();
KoenKahlman 1:1648ce825c31 380 if(iter % 100 == 0){
KoenKahlman 1:1648ce825c31 381 displayTerminalStatus();
KoenKahlman 1:1648ce825c31 382 }
KoenKahlman 1:1648ce825c31 383 }
KoenKahlman 1:1648ce825c31 384
KoenKahlman 1:1648ce825c31 385 //TRAIN COMMAND
KoenKahlman 1:1648ce825c31 386 void trainCommand(){
KoenKahlman 1:1648ce825c31 387 float Vin = potentio.read() * 3.3;
KoenKahlman 1:1648ce825c31 388 speedsetting = floor(Vin/0.22); //0.22 = 3.3/15
KoenKahlman 1:1648ce825c31 389
KoenKahlman 1:1648ce825c31 390 if(boarding1 || boarding2){
KoenKahlman 1:1648ce825c31 391 if(station1.read() > stationWaitTime){ //timer works? TODO
KoenKahlman 1:1648ce825c31 392 boarding1 = false;
KoenKahlman 1:1648ce825c31 393 led1 = 0;
KoenKahlman 1:1648ce825c31 394 station1.stop();
KoenKahlman 1:1648ce825c31 395 }
KoenKahlman 1:1648ce825c31 396
KoenKahlman 1:1648ce825c31 397 if(station2.read() > stationWaitTime){
KoenKahlman 1:1648ce825c31 398 boarding2 = false;
KoenKahlman 1:1648ce825c31 399 led2 = 0;
KoenKahlman 1:1648ce825c31 400 station2.stop();
KoenKahlman 1:1648ce825c31 401 }
KoenKahlman 1:1648ce825c31 402 }
KoenKahlman 1:1648ce825c31 403 speed1 = boarding1?0:speedsetting;
KoenKahlman 1:1648ce825c31 404 speed2 = boarding2?0:speedsetting;
KoenKahlman 1:1648ce825c31 405
KoenKahlman 1:1648ce825c31 406 if(next1 == next2 && !collision){
KoenKahlman 1:1648ce825c31 407 headCollision();
KoenKahlman 1:1648ce825c31 408 collision = true;
KoenKahlman 1:1648ce825c31 409 } else if ((next1 == curr2 || next2 == curr1) && !collision){
KoenKahlman 1:1648ce825c31 410 sideCollision();
KoenKahlman 1:1648ce825c31 411 collision = true;
KoenKahlman 1:1648ce825c31 412 } else if (isXCollision() && !collision){
KoenKahlman 1:1648ce825c31 413 xCollision();
KoenKahlman 1:1648ce825c31 414 collision = true;
KoenKahlman 1:1648ce825c31 415 } else if (next1 == prev2 && !command1){
KoenKahlman 1:1648ce825c31 416 tailCollision(1);
KoenKahlman 1:1648ce825c31 417 command1 = true;
KoenKahlman 1:1648ce825c31 418 } else if (next2 == prev1 && !command1){
KoenKahlman 1:1648ce825c31 419 tailCollision(2);
KoenKahlman 1:1648ce825c31 420 command1 = true;
KoenKahlman 1:1648ce825c31 421 } else if (curr1 == 2 && !command1){
KoenKahlman 1:1648ce825c31 422 atStation2(1);
KoenKahlman 1:1648ce825c31 423 command1 = true;
KoenKahlman 1:1648ce825c31 424 } else if (curr1 == 6 && !command1){
KoenKahlman 1:1648ce825c31 425 atSwitch3(1);
KoenKahlman 1:1648ce825c31 426 command1 = true;
KoenKahlman 1:1648ce825c31 427 } else if (curr1 == 8 && !command1){
KoenKahlman 1:1648ce825c31 428 atSwitch4(1);
KoenKahlman 1:1648ce825c31 429 command1 = true;
KoenKahlman 1:1648ce825c31 430 } else if (curr1 == 12 && !command1){
KoenKahlman 1:1648ce825c31 431 atStation12(1);
KoenKahlman 1:1648ce825c31 432 command1 = true;
KoenKahlman 1:1648ce825c31 433 } else if (curr2 == 2 && !command2){
KoenKahlman 1:1648ce825c31 434 atStation2(1);
KoenKahlman 1:1648ce825c31 435 command2 = true;
KoenKahlman 1:1648ce825c31 436 } else if (curr2 == 6 && !command2){
KoenKahlman 1:1648ce825c31 437 atSwitch3(2);
KoenKahlman 1:1648ce825c31 438 command2 = true;
KoenKahlman 1:1648ce825c31 439 } else if (curr2 == 8 && !command2){
KoenKahlman 1:1648ce825c31 440 atSwitch4(2);
KoenKahlman 1:1648ce825c31 441 command2 = true;
KoenKahlman 1:1648ce825c31 442 } else if (curr2 == 12 && !command2){
KoenKahlman 1:1648ce825c31 443 atStation12(2);
KoenKahlman 1:1648ce825c31 444 command2 = true;
KoenKahlman 1:1648ce825c31 445 }
KoenKahlman 1:1648ce825c31 446
KoenKahlman 1:1648ce825c31 447 driveTrain(2);
KoenKahlman 1:1648ce825c31 448 driveTrain(1);
KoenKahlman 1:1648ce825c31 449
KoenKahlman 1:1648ce825c31 450 /*
KoenKahlman 1:1648ce825c31 451 else if (!command1 && (curr1 == 9 && next1 == 3) || (curr1 == 5 && next1 == 11)){
KoenKahlman 1:1648ce825c31 452 loopDeltaCrossing(1);
KoenKahlman 1:1648ce825c31 453 command1 = true;
KoenKahlman 1:1648ce825c31 454 } else if (!command2 && (curr2 == 9 && next2 == 3) || (curr2 == 5 && next2 == 11)){
KoenKahlman 1:1648ce825c31 455 loopDeltaCrossing(2);
KoenKahlman 1:1648ce825c31 456 command2 = true;
KoenKahlman 1:1648ce825c31 457 }
KoenKahlman 1:1648ce825c31 458 */
KoenKahlman 1:1648ce825c31 459 }
KoenKahlman 1:1648ce825c31 460
KoenKahlman 1:1648ce825c31 461 bool isXCollision(){
KoenKahlman 1:1648ce825c31 462 return (curr1 == 3 && next1 == 9) && (next2 == 5 || next2 == 11)
KoenKahlman 1:1648ce825c31 463 || (curr1 == 5 && next1 == 11) && (next2 == 3 || next2 == 9)
KoenKahlman 1:1648ce825c31 464 || (curr1 == 9 && next1 == 3) && (next2 == 5 || next2 == 11)
KoenKahlman 1:1648ce825c31 465 || (curr1 == 11 && next1 == 5) && (next2 == 3 || next2 == 9)
KoenKahlman 1:1648ce825c31 466 || (curr2 == 3 && next2 == 9) && (next1 == 5 || next1 == 11)
KoenKahlman 1:1648ce825c31 467 || (curr2 == 5 && next2 == 11) && (next1 == 3 || next1 == 9)
KoenKahlman 1:1648ce825c31 468 || (curr2 == 9 && next2 == 3) && (next1 == 5 || next1 == 11)
KoenKahlman 1:1648ce825c31 469 || (curr2 == 11 && next2 == 5) && (next1 == 3 || next1 == 9);
KoenKahlman 1:1648ce825c31 470 }
KoenKahlman 1:1648ce825c31 471
KoenKahlman 1:1648ce825c31 472 void stopTrain(int train){
KoenKahlman 1:1648ce825c31 473 if(train == 1){
KoenKahlman 1:1648ce825c31 474 DCC_send_command(DCCaddress_train1, DCCinst_estop, 10);
KoenKahlman 1:1648ce825c31 475 pc.printf("STATUS: Train 1 stopped\n\r");
KoenKahlman 1:1648ce825c31 476 } else { //train == 2
KoenKahlman 1:1648ce825c31 477 DCC_send_command(DCCaddress_train2, DCCinst_estop, 10);
KoenKahlman 1:1648ce825c31 478 pc.printf("STATUS: Train 2 stopped\n\r");
KoenKahlman 1:1648ce825c31 479 }
KoenKahlman 1:1648ce825c31 480 }
KoenKahlman 1:1648ce825c31 481
KoenKahlman 1:1648ce825c31 482 void reverseTrain(int train){ //always stop before reversing
KoenKahlman 1:1648ce825c31 483 if(train == 1){
KoenKahlman 1:1648ce825c31 484 int stupid;
KoenKahlman 1:1648ce825c31 485 dir1 = !dir1;
KoenKahlman 1:1648ce825c31 486 stupid = next1;
KoenKahlman 1:1648ce825c31 487 next1 = curr1;
KoenKahlman 1:1648ce825c31 488 curr1 = stupid;
KoenKahlman 1:1648ce825c31 489 prev1 = predictNextMarker(curr1, next1);
KoenKahlman 1:1648ce825c31 490 pc.printf("STATUS: Train 1 reversed\n\r");
KoenKahlman 1:1648ce825c31 491 } else { //train == 2
KoenKahlman 1:1648ce825c31 492 int swap;
KoenKahlman 1:1648ce825c31 493 dir2 = !dir2;
KoenKahlman 1:1648ce825c31 494 swap = next2;
KoenKahlman 1:1648ce825c31 495 next2 = curr2;
KoenKahlman 1:1648ce825c31 496 curr2 = swap;
KoenKahlman 1:1648ce825c31 497 prev2 = predictNextMarker(curr2, next2);
KoenKahlman 1:1648ce825c31 498 pc.printf("STATUS: Train 2 reversed\n\r");
KoenKahlman 1:1648ce825c31 499 }
KoenKahlman 1:1648ce825c31 500 }
KoenKahlman 1:1648ce825c31 501
KoenKahlman 1:1648ce825c31 502 void driveTrain(int train){
KoenKahlman 1:1648ce825c31 503 if(train == 1){
KoenKahlman 1:1648ce825c31 504 DCC_send_command(DCCaddress_train1, trainInstruction(dir1, light1, speed1), 25);
KoenKahlman 1:1648ce825c31 505 } else { //train == 2
KoenKahlman 1:1648ce825c31 506 DCC_send_command(DCCaddress_train2, trainInstruction(dir2, light2, speed2), 25);
KoenKahlman 1:1648ce825c31 507 }
KoenKahlman 1:1648ce825c31 508 }
KoenKahlman 1:1648ce825c31 509
KoenKahlman 1:1648ce825c31 510 void atStation2(int train){
KoenKahlman 1:1648ce825c31 511 if(train == 1){
KoenKahlman 1:1648ce825c31 512 if(next1 != 22){
KoenKahlman 1:1648ce825c31 513 pc.printf("STATUS: train 1 reached switch 2\n\r");
KoenKahlman 1:1648ce825c31 514 stopTrain(1);
KoenKahlman 1:1648ce825c31 515 if(curr2 == 6 || next2 == 6){
KoenKahlman 1:1648ce825c31 516 switch2 = true;
KoenKahlman 1:1648ce825c31 517 } else if(curr2 == 9 || next2 == 9){
KoenKahlman 1:1648ce825c31 518 switch2 = false;
KoenKahlman 1:1648ce825c31 519 } else { //other train far away
KoenKahlman 1:1648ce825c31 520 switch2 = rand()%2;
KoenKahlman 1:1648ce825c31 521 }
KoenKahlman 1:1648ce825c31 522 changeSwitch2(switch2);
KoenKahlman 1:1648ce825c31 523 }
KoenKahlman 1:1648ce825c31 524 atStation(1);
KoenKahlman 1:1648ce825c31 525 } else { //train == 2
KoenKahlman 1:1648ce825c31 526 if(next2 != 22){
KoenKahlman 1:1648ce825c31 527 pc.printf("STATUS: train 2 reached switch 2\n\r");
KoenKahlman 1:1648ce825c31 528 stopTrain(2);
KoenKahlman 1:1648ce825c31 529 if(curr1 == 6 || next1 == 6){
KoenKahlman 1:1648ce825c31 530 //switch2 = true;
KoenKahlman 1:1648ce825c31 531 } else if(curr1 == 9 || next1 == 9){
KoenKahlman 1:1648ce825c31 532 //switch2 = false;
KoenKahlman 1:1648ce825c31 533 } else { //other train far away
KoenKahlman 1:1648ce825c31 534 switch2 = rand()%2;
KoenKahlman 1:1648ce825c31 535 }
KoenKahlman 1:1648ce825c31 536 changeSwitch2(switch2);
KoenKahlman 1:1648ce825c31 537 }
KoenKahlman 1:1648ce825c31 538 atStation(2);
KoenKahlman 1:1648ce825c31 539 }
KoenKahlman 1:1648ce825c31 540 }
KoenKahlman 1:1648ce825c31 541
KoenKahlman 1:1648ce825c31 542 void atStation12(int train){
KoenKahlman 1:1648ce825c31 543 if(train == 1){
KoenKahlman 1:1648ce825c31 544 if(next1 != 22){
KoenKahlman 1:1648ce825c31 545 pc.printf("STATUS: train 1 reached switch 1\n\r");
KoenKahlman 1:1648ce825c31 546 stopTrain(1);
KoenKahlman 1:1648ce825c31 547 if(curr2 == 8 || next2 == 8){
KoenKahlman 1:1648ce825c31 548 //switch1 = true;
KoenKahlman 1:1648ce825c31 549 } else if(curr2 == 5 || next2 == 5){
KoenKahlman 1:1648ce825c31 550 //switch1 = false;
KoenKahlman 1:1648ce825c31 551 } else { //other train far away
KoenKahlman 1:1648ce825c31 552 switch1 = rand()%2;
KoenKahlman 1:1648ce825c31 553 }
KoenKahlman 1:1648ce825c31 554 changeSwitch1(switch1);
KoenKahlman 1:1648ce825c31 555 }
KoenKahlman 1:1648ce825c31 556 atStation(1);
KoenKahlman 1:1648ce825c31 557 } else { //train == 2
KoenKahlman 1:1648ce825c31 558 if(next2 != 22){
KoenKahlman 1:1648ce825c31 559 pc.printf("STATUS: train 2 reached switch 1\n\r");
KoenKahlman 1:1648ce825c31 560 stopTrain(2);
KoenKahlman 1:1648ce825c31 561 if(curr1 == 8 || next1 == 8){
KoenKahlman 1:1648ce825c31 562 //switch1 = true;
KoenKahlman 1:1648ce825c31 563 } else if(curr1 == 5 || next1 == 5){
KoenKahlman 1:1648ce825c31 564 //switch1 = false;
KoenKahlman 1:1648ce825c31 565 } else { //other train far away
KoenKahlman 1:1648ce825c31 566 switch1 = rand()%2;
KoenKahlman 1:1648ce825c31 567 }
KoenKahlman 1:1648ce825c31 568 changeSwitch1(switch1);
KoenKahlman 1:1648ce825c31 569 }
KoenKahlman 1:1648ce825c31 570 atStation(2);
KoenKahlman 1:1648ce825c31 571 }
KoenKahlman 1:1648ce825c31 572 }
KoenKahlman 1:1648ce825c31 573
KoenKahlman 1:1648ce825c31 574 void atStation(int train){
KoenKahlman 1:1648ce825c31 575 if(train == 1){
KoenKahlman 1:1648ce825c31 576 pc.printf("STATUS: Train 1 reached station!\n\r");
KoenKahlman 1:1648ce825c31 577 boxled1 = 1;
KoenKahlman 1:1648ce825c31 578 speed1 = 0;
KoenKahlman 1:1648ce825c31 579 station1.start();
KoenKahlman 1:1648ce825c31 580 boarding1 = true;
KoenKahlman 1:1648ce825c31 581 } else { //train == 2
KoenKahlman 1:1648ce825c31 582 pc.printf("STATUS: Train 2 reached station!\n\r");
KoenKahlman 1:1648ce825c31 583 boxled2 = 1;
KoenKahlman 1:1648ce825c31 584 speed2 = 0;
KoenKahlman 1:1648ce825c31 585 station2.start();
KoenKahlman 1:1648ce825c31 586 boarding2 = true;
KoenKahlman 1:1648ce825c31 587 }
KoenKahlman 1:1648ce825c31 588 }
KoenKahlman 1:1648ce825c31 589
KoenKahlman 1:1648ce825c31 590 void atSwitch3(int train){
KoenKahlman 1:1648ce825c31 591 if(train == 1){
KoenKahlman 1:1648ce825c31 592 if(next1 == 7){ return; }
KoenKahlman 1:1648ce825c31 593 pc.printf("STATUS: train 1 reached switch 3\n\r");
KoenKahlman 1:1648ce825c31 594 stopTrain(1);
KoenKahlman 1:1648ce825c31 595 if(curr2 == 2 || next2 == 2){
KoenKahlman 1:1648ce825c31 596 //switch3 = true; TODO complicated
KoenKahlman 1:1648ce825c31 597 } else if(curr2 == 11 || next2 == 11){
KoenKahlman 1:1648ce825c31 598 //switch3 = false;
KoenKahlman 1:1648ce825c31 599 } else { //other train far away
KoenKahlman 1:1648ce825c31 600 switch3 = rand()%2;
KoenKahlman 1:1648ce825c31 601 }
KoenKahlman 1:1648ce825c31 602 changeSwitch3(switch3);
KoenKahlman 1:1648ce825c31 603 } else { //train == 2
KoenKahlman 1:1648ce825c31 604 if(next2 == 7){ return; }
KoenKahlman 1:1648ce825c31 605 pc.printf("STATUS: train 2 reached switch 3\n\r");
KoenKahlman 1:1648ce825c31 606 stopTrain(2);
KoenKahlman 1:1648ce825c31 607 if(curr1 == 2 || next1 == 2){
KoenKahlman 1:1648ce825c31 608 //switch3 = true;
KoenKahlman 1:1648ce825c31 609 } else if(curr1 == 11 || next1 == 11){
KoenKahlman 1:1648ce825c31 610 //switch3 = false;
KoenKahlman 1:1648ce825c31 611 } else { //other train far away
KoenKahlman 1:1648ce825c31 612 switch3 = rand()%2;
KoenKahlman 1:1648ce825c31 613 }
KoenKahlman 1:1648ce825c31 614 changeSwitch3(switch3);
KoenKahlman 1:1648ce825c31 615 }
KoenKahlman 1:1648ce825c31 616 }
KoenKahlman 1:1648ce825c31 617
KoenKahlman 1:1648ce825c31 618 void atSwitch4(int train){
KoenKahlman 1:1648ce825c31 619 if(train == 1){
KoenKahlman 1:1648ce825c31 620 if(next1 == 7){ return; }
KoenKahlman 1:1648ce825c31 621 pc.printf("STATUS: train 1 reached switch 4\n\r");
KoenKahlman 1:1648ce825c31 622 stopTrain(1);
KoenKahlman 1:1648ce825c31 623 if(curr2 == 2 || next2 == 2){
KoenKahlman 1:1648ce825c31 624 //switch4 = true;
KoenKahlman 1:1648ce825c31 625 } else if(curr2 == 11 || next2 == 11){
KoenKahlman 1:1648ce825c31 626 //switch4 = false;
KoenKahlman 1:1648ce825c31 627 } else { //other train far away
KoenKahlman 1:1648ce825c31 628 switch4 = rand()%2;
KoenKahlman 1:1648ce825c31 629 }
KoenKahlman 1:1648ce825c31 630 changeSwitch4(switch4);
KoenKahlman 1:1648ce825c31 631 } else { //train == 2
KoenKahlman 1:1648ce825c31 632 if(next2 == 7){ return; }
KoenKahlman 1:1648ce825c31 633 pc.printf("STATUS: train 2 reached switch 4\n\r");
KoenKahlman 1:1648ce825c31 634 stopTrain(2);
KoenKahlman 1:1648ce825c31 635 if(curr1 == 2 || next1 == 2){
KoenKahlman 1:1648ce825c31 636 //switch4 = true;
KoenKahlman 1:1648ce825c31 637 } else if(curr1 == 11 || next1 == 11){
KoenKahlman 1:1648ce825c31 638 //switch4 = false;
KoenKahlman 1:1648ce825c31 639 } else { //other train far away
KoenKahlman 1:1648ce825c31 640 switch4 = rand()%2;
KoenKahlman 1:1648ce825c31 641 }
KoenKahlman 1:1648ce825c31 642 changeSwitch4(switch4);
KoenKahlman 1:1648ce825c31 643 }
KoenKahlman 1:1648ce825c31 644 }
KoenKahlman 1:1648ce825c31 645
KoenKahlman 1:1648ce825c31 646 void headCollision(void){
KoenKahlman 1:1648ce825c31 647 buzzer = 1;
KoenKahlman 1:1648ce825c31 648 pc.printf("WARNING: inbound head collision\n\r");
KoenKahlman 1:1648ce825c31 649 stopTrain(1);
KoenKahlman 1:1648ce825c31 650 stopTrain(2);
KoenKahlman 1:1648ce825c31 651 reverseTrain(1);
KoenKahlman 1:1648ce825c31 652 reverseTrain(2);
KoenKahlman 1:1648ce825c31 653 buzzer = 0;
KoenKahlman 1:1648ce825c31 654 }
KoenKahlman 1:1648ce825c31 655
KoenKahlman 1:1648ce825c31 656 void sideCollision(void){
KoenKahlman 1:1648ce825c31 657 buzzer = 1;
KoenKahlman 1:1648ce825c31 658 pc.printf("WARNING: possible side collision\n\r");
KoenKahlman 1:1648ce825c31 659 stopTrain(1);
KoenKahlman 1:1648ce825c31 660 stopTrain(2);
KoenKahlman 1:1648ce825c31 661 reverseTrain(1);
KoenKahlman 1:1648ce825c31 662 reverseTrain(2);
KoenKahlman 1:1648ce825c31 663 buzzer = 0;
KoenKahlman 1:1648ce825c31 664 }
KoenKahlman 1:1648ce825c31 665
KoenKahlman 1:1648ce825c31 666 void xCollision(void){
KoenKahlman 1:1648ce825c31 667 buzzer = 1;
KoenKahlman 1:1648ce825c31 668 pc.printf("WARNING: possible cross collision\n\r");
KoenKahlman 1:1648ce825c31 669 stopTrain(1);
KoenKahlman 1:1648ce825c31 670 stopTrain(2);
KoenKahlman 1:1648ce825c31 671 reverseTrain(1);
KoenKahlman 1:1648ce825c31 672 reverseTrain(2);
KoenKahlman 1:1648ce825c31 673 buzzer = 0;
KoenKahlman 1:1648ce825c31 674 }
KoenKahlman 1:1648ce825c31 675
KoenKahlman 1:1648ce825c31 676 void tailCollision(int backTrain){
KoenKahlman 1:1648ce825c31 677 buzzer = 1;
KoenKahlman 1:1648ce825c31 678 pc.printf("WARNING: possible tail collision\n\r");
KoenKahlman 1:1648ce825c31 679 stopTrain(backTrain);
KoenKahlman 1:1648ce825c31 680 reverseTrain(backTrain);
KoenKahlman 1:1648ce825c31 681 buzzer = 0;
KoenKahlman 1:1648ce825c31 682 }
KoenKahlman 1:1648ce825c31 683 /*
KoenKahlman 1:1648ce825c31 684 void loopDeltaCrossing(int train){
KoenKahlman 1:1648ce825c31 685 buzzer = 1;
KoenKahlman 1:1648ce825c31 686 pc.printf("WARNING: attempting to exit the loop\n\r");
KoenKahlman 1:1648ce825c31 687 if(train == 1){
KoenKahlman 1:1648ce825c31 688 if(curr2 != 13 && curr2 != 0 && curr2 != 1 && curr2 != 2){
KoenKahlman 1:1648ce825c31 689 stopTrain(1);
KoenKahlman 1:1648ce825c31 690 reverseTrain(1);
KoenKahlman 1:1648ce825c31 691 }
KoenKahlman 1:1648ce825c31 692 //if train 2 in loop
KoenKahlman 1:1648ce825c31 693 //stop, reverse
KoenKahlman 1:1648ce825c31 694 } else { //train == 2
KoenKahlman 1:1648ce825c31 695 //if train 1 in loop
KoenKahlman 1:1648ce825c31 696 //stop, reverse
KoenKahlman 1:1648ce825c31 697 if(curr1 != 13 && curr1 != 0 && curr1 != 1 && curr1 != 2){
KoenKahlman 1:1648ce825c31 698 stopTrain(1);
KoenKahlman 1:1648ce825c31 699 reverseTrain(1);
KoenKahlman 1:1648ce825c31 700 }
KoenKahlman 1:1648ce825c31 701 }
KoenKahlman 1:1648ce825c31 702 buzzer = 0;
KoenKahlman 1:1648ce825c31 703 }
KoenKahlman 1:1648ce825c31 704 */
KoenKahlman 0:9c82986d7cb9 705
KoenKahlman 0:9c82986d7cb9 706
KoenKahlman 0:9c82986d7cb9 707
KoenKahlman 0:9c82986d7cb9 708
KoenKahlman 0:9c82986d7cb9 709 //TRACK COMMANDS
KoenKahlman 0:9c82986d7cb9 710 //code from the tutorial, sends a message to the track through the train pin
KoenKahlman 0:9c82986d7cb9 711 void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count){
KoenKahlman 0:9c82986d7cb9 712 switch(address){
KoenKahlman 1:1648ce825c31 713 case DCCaddress_train1: led1 = 1; break;
KoenKahlman 1:1648ce825c31 714 case DCCaddress_train2: led2 = 1; break;
KoenKahlman 1:1648ce825c31 715 case DCCaddress_decoder: led3 = 1; break;
KoenKahlman 0:9c82986d7cb9 716 }
KoenKahlman 0:9c82986d7cb9 717
KoenKahlman 0:9c82986d7cb9 718 unsigned __int64 command = 0x0000000000000000; // __int64 is the 64-bit integer type
KoenKahlman 0:9c82986d7cb9 719 unsigned __int64 temp_command = 0x0000000000000000;
KoenKahlman 0:9c82986d7cb9 720 unsigned __int64 prefix = 0x3FFF; // 14 "1" bits needed at start
KoenKahlman 0:9c82986d7cb9 721 unsigned int error = 0x00; //error byte
KoenKahlman 0:9c82986d7cb9 722 //calculate error detection byte with xor
KoenKahlman 0:9c82986d7cb9 723 error = address ^ inst;
KoenKahlman 0:9c82986d7cb9 724 //combine packet bits in basic DCC format
KoenKahlman 0:9c82986d7cb9 725 command = (prefix<<28)|(address<<19)|(inst<<10)|((error)<<1)|0x01;
KoenKahlman 0:9c82986d7cb9 726 //printf("\n\r %llx \n\r",command);
KoenKahlman 0:9c82986d7cb9 727 int i=0;
KoenKahlman 0:9c82986d7cb9 728 //repeat DCC command lots of times
KoenKahlman 0:9c82986d7cb9 729 while(i < repeat_count) {
KoenKahlman 0:9c82986d7cb9 730 temp_command = command;
KoenKahlman 0:9c82986d7cb9 731 //loops through packet bits encoding and sending out digital pulses for a DCC command
KoenKahlman 0:9c82986d7cb9 732 for (int j=0; j<64; j++) {
KoenKahlman 0:9c82986d7cb9 733 if((temp_command&0x8000000000000000)==0) { //test packet bit
KoenKahlman 0:9c82986d7cb9 734 //send data for a "0" bit
KoenKahlman 0:9c82986d7cb9 735 data=0;
KoenKahlman 0:9c82986d7cb9 736 wait_us(100);
KoenKahlman 0:9c82986d7cb9 737 data=1;
KoenKahlman 0:9c82986d7cb9 738 wait_us(100);
KoenKahlman 0:9c82986d7cb9 739 //printf("0011");
KoenKahlman 0:9c82986d7cb9 740 } else {
KoenKahlman 0:9c82986d7cb9 741 //send data for a "1"bit
KoenKahlman 0:9c82986d7cb9 742 data=0;
KoenKahlman 0:9c82986d7cb9 743 wait_us(58);
KoenKahlman 0:9c82986d7cb9 744 data=1;
KoenKahlman 0:9c82986d7cb9 745 wait_us(58);
KoenKahlman 0:9c82986d7cb9 746 //printf("01");
KoenKahlman 0:9c82986d7cb9 747 }
KoenKahlman 0:9c82986d7cb9 748 // next bit in packet
KoenKahlman 0:9c82986d7cb9 749 temp_command = temp_command<<1;
KoenKahlman 0:9c82986d7cb9 750 }
KoenKahlman 0:9c82986d7cb9 751 i++;
KoenKahlman 0:9c82986d7cb9 752 }
KoenKahlman 0:9c82986d7cb9 753
KoenKahlman 0:9c82986d7cb9 754 led1 = 0;
KoenKahlman 0:9c82986d7cb9 755 led2 = 0;
KoenKahlman 1:1648ce825c31 756 led3 = 0;
KoenKahlman 0:9c82986d7cb9 757 }
KoenKahlman 0:9c82986d7cb9 758
KoenKahlman 0:9c82986d7cb9 759 void changeSwitch1(bool inwards){
KoenKahlman 0:9c82986d7cb9 760 if(inwards){
KoenKahlman 0:9c82986d7cb9 761 DCC_send_command(DCCaddress_decoder, DCCswitch1, 1);
KoenKahlman 1:1648ce825c31 762 pc.printf("STATUS: Switch 1 changed inward\n\r");
KoenKahlman 0:9c82986d7cb9 763 } else {
KoenKahlman 0:9c82986d7cb9 764 DCC_send_command(DCCaddress_decoder, DCCswitch1, 1);
KoenKahlman 0:9c82986d7cb9 765 DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
KoenKahlman 1:1648ce825c31 766 pc.printf("STATUS: Switch 1 changed outward\n\r");
KoenKahlman 0:9c82986d7cb9 767 }
KoenKahlman 1:1648ce825c31 768 updateTrainPredictions();
KoenKahlman 0:9c82986d7cb9 769 }
KoenKahlman 0:9c82986d7cb9 770
KoenKahlman 0:9c82986d7cb9 771 void changeSwitch2(bool inwards){
KoenKahlman 0:9c82986d7cb9 772 if(inwards){
KoenKahlman 0:9c82986d7cb9 773 DCC_send_command(DCCaddress_decoder, DCCswitch2, 1);
KoenKahlman 1:1648ce825c31 774 pc.printf("STATUS: Switch 2 changed inward\n\r");
KoenKahlman 0:9c82986d7cb9 775 } else {
KoenKahlman 0:9c82986d7cb9 776 DCC_send_command(DCCaddress_decoder, DCCswitch2, 1);
KoenKahlman 0:9c82986d7cb9 777 DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
KoenKahlman 1:1648ce825c31 778 pc.printf("STATUS: Switch 2 changed outward\n\r");
KoenKahlman 1:1648ce825c31 779 }
KoenKahlman 1:1648ce825c31 780 updateTrainPredictions();
KoenKahlman 0:9c82986d7cb9 781 }
KoenKahlman 0:9c82986d7cb9 782
KoenKahlman 0:9c82986d7cb9 783 void changeSwitch3(bool inwards){
KoenKahlman 0:9c82986d7cb9 784 if(inwards){
KoenKahlman 0:9c82986d7cb9 785 DCC_send_command(DCCaddress_decoder, DCCswitch3, 1);
KoenKahlman 0:9c82986d7cb9 786 DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1); //switch 3 has reverse behavior
KoenKahlman 1:1648ce825c31 787 pc.printf("STATUS: Switch 3 changed inward\n\r");
KoenKahlman 0:9c82986d7cb9 788 } else {
KoenKahlman 0:9c82986d7cb9 789 DCC_send_command(DCCaddress_decoder, DCCswitch3, 1);
KoenKahlman 1:1648ce825c31 790 pc.printf("STATUS: Switch 3 changed outward\n\r");
KoenKahlman 1:1648ce825c31 791 }
KoenKahlman 1:1648ce825c31 792 updateTrainPredictions();
KoenKahlman 0:9c82986d7cb9 793 }
KoenKahlman 0:9c82986d7cb9 794
KoenKahlman 0:9c82986d7cb9 795 void changeSwitch4(bool inwards){
KoenKahlman 0:9c82986d7cb9 796 if(inwards){
KoenKahlman 0:9c82986d7cb9 797 DCC_send_command(DCCaddress_decoder, DCCswitch4, 1);
KoenKahlman 1:1648ce825c31 798 pc.printf("STATUS: Switch 4 changed inward\n\r");
KoenKahlman 0:9c82986d7cb9 799 } else {
KoenKahlman 0:9c82986d7cb9 800 DCC_send_command(DCCaddress_decoder, DCCswitch4, 1);
KoenKahlman 0:9c82986d7cb9 801 DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
KoenKahlman 1:1648ce825c31 802 pc.printf("STATUS: Switch 4 changed outward\n\r");
KoenKahlman 1:1648ce825c31 803 }
KoenKahlman 1:1648ce825c31 804 updateTrainPredictions();
KoenKahlman 1:1648ce825c31 805 }
KoenKahlman 1:1648ce825c31 806
KoenKahlman 1:1648ce825c31 807 void updateTrainPredictions(){
KoenKahlman 1:1648ce825c31 808 next1 = predictNextMarker(prev1, curr1);
KoenKahlman 1:1648ce825c31 809 next2 = predictNextMarker(prev2, curr2);
KoenKahlman 0:9c82986d7cb9 810 }
KoenKahlman 0:9c82986d7cb9 811
KoenKahlman 0:9c82986d7cb9 812 unsigned int trainInstruction(bool direction, bool light, int speed){
KoenKahlman 0:9c82986d7cb9 813 unsigned int result = 0x40; //0100 0000
KoenKahlman 0:9c82986d7cb9 814 if(direction){
KoenKahlman 0:9c82986d7cb9 815 result += 0x20; //0010 0000
KoenKahlman 0:9c82986d7cb9 816 }
KoenKahlman 0:9c82986d7cb9 817 if(light){
KoenKahlman 0:9c82986d7cb9 818 result += 0x10; //0001 0000
KoenKahlman 0:9c82986d7cb9 819 }
KoenKahlman 0:9c82986d7cb9 820 if(speed>0 && speed<15){
KoenKahlman 0:9c82986d7cb9 821 result += speed + 1;
KoenKahlman 0:9c82986d7cb9 822 }
KoenKahlman 0:9c82986d7cb9 823 return result;
KoenKahlman 0:9c82986d7cb9 824 }
KoenKahlman 0:9c82986d7cb9 825
KoenKahlman 0:9c82986d7cb9 826 //INTERPRET INTERRUPT SIGNALS
KoenKahlman 0:9c82986d7cb9 827 //INTERRUPTS
KoenKahlman 0:9c82986d7cb9 828 //code from the tutorial, initializes MCP and sets it up for interrupts
KoenKahlman 0:9c82986d7cb9 829 void init_mcp() {
KoenKahlman 0:9c82986d7cb9 830 // Initialisation of MCP registers, documentation on registers is available at
KoenKahlman 0:9c82986d7cb9 831 //Niels/Abel/Robert/Natalia
KoenKahlman 0:9c82986d7cb9 832 mcp = new MCP23017(i2c, 0x40);
KoenKahlman 0:9c82986d7cb9 833 mcp->_write(IODIRA, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 834 mcp->_write(IODIRB, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 835 mcp->_write(IPOLA, (unsigned char )0x00);
KoenKahlman 0:9c82986d7cb9 836 mcp->_write(IPOLB, (unsigned char )0x00);
KoenKahlman 0:9c82986d7cb9 837 mcp->_write(DEFVALA, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 838 mcp->_write(DEFVALB, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 839 mcp->_write(INTCONA, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 840 mcp->_write(INTCONB, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 841 mcp->_write(IOCONA, (unsigned char )0x2);
KoenKahlman 0:9c82986d7cb9 842 mcp->_write(IOCONB, (unsigned char )0x2);
KoenKahlman 0:9c82986d7cb9 843 mcp->_write(GPPUA, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 844 mcp->_write(GPPUB, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 845
KoenKahlman 0:9c82986d7cb9 846 // Clear current interrupts
KoenKahlman 0:9c82986d7cb9 847 mcp->_read(GPIOA);
KoenKahlman 0:9c82986d7cb9 848 mcp->_read(GPIOB);
KoenKahlman 0:9c82986d7cb9 849 // Register callbacks
KoenKahlman 0:9c82986d7cb9 850 int0.fall(&on_int0_change);
KoenKahlman 0:9c82986d7cb9 851 int1.fall(&on_int1_change);
KoenKahlman 0:9c82986d7cb9 852 // Enable interrupts on the MCP
KoenKahlman 0:9c82986d7cb9 853 mcp->_write(GPINTENA, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 854 mcp->_write(GPINTENB, (unsigned char )0xff);
KoenKahlman 0:9c82986d7cb9 855
KoenKahlman 0:9c82986d7cb9 856 int0flag = false;
KoenKahlman 0:9c82986d7cb9 857 int1flag = false;
KoenKahlman 0:9c82986d7cb9 858 }
KoenKahlman 0:9c82986d7cb9 859
KoenKahlman 0:9c82986d7cb9 860 void on_int0_change() {
KoenKahlman 0:9c82986d7cb9 861 int0flag = true;
KoenKahlman 0:9c82986d7cb9 862 }
KoenKahlman 0:9c82986d7cb9 863
KoenKahlman 0:9c82986d7cb9 864 void on_int1_change() {
KoenKahlman 0:9c82986d7cb9 865 int1flag = true;
KoenKahlman 0:9c82986d7cb9 866 }
KoenKahlman 0:9c82986d7cb9 867
KoenKahlman 0:9c82986d7cb9 868 std::list<int> circuit0markers(int sensor_data){
KoenKahlman 0:9c82986d7cb9 869 std::list<int> result;
KoenKahlman 0:9c82986d7cb9 870 sensor_data = ~sensor_data;
KoenKahlman 0:9c82986d7cb9 871 if((sensor_data & 0b00000001) == 0b00000001){
KoenKahlman 0:9c82986d7cb9 872 result.push_back(0);
KoenKahlman 0:9c82986d7cb9 873 }
KoenKahlman 0:9c82986d7cb9 874 if((sensor_data & 0b00000010) == 0b00000010){
KoenKahlman 0:9c82986d7cb9 875 result.push_back(1);
KoenKahlman 0:9c82986d7cb9 876 }
KoenKahlman 0:9c82986d7cb9 877 if((sensor_data & 0b00000100) == 0b00000100){
KoenKahlman 0:9c82986d7cb9 878 result.push_back(2);
KoenKahlman 0:9c82986d7cb9 879 }
KoenKahlman 0:9c82986d7cb9 880 if((sensor_data & 0b00001000) == 0b00001000){
KoenKahlman 0:9c82986d7cb9 881 result.push_back(3);
KoenKahlman 0:9c82986d7cb9 882 }
KoenKahlman 0:9c82986d7cb9 883 if((sensor_data & 0b00010000) == 0b00010000){
KoenKahlman 0:9c82986d7cb9 884 result.push_back(4);
KoenKahlman 0:9c82986d7cb9 885 }
KoenKahlman 0:9c82986d7cb9 886 if((sensor_data & 0b00100000) == 0b00100000){
KoenKahlman 0:9c82986d7cb9 887 result.push_back(5);
KoenKahlman 0:9c82986d7cb9 888 }
KoenKahlman 0:9c82986d7cb9 889 if((sensor_data & 0b01000000) == 0b01000000){
KoenKahlman 0:9c82986d7cb9 890 result.push_back(6);
KoenKahlman 0:9c82986d7cb9 891 }
KoenKahlman 0:9c82986d7cb9 892 if((sensor_data & 0b10000000) == 0b10000000){
KoenKahlman 0:9c82986d7cb9 893 result.push_back(7);
KoenKahlman 0:9c82986d7cb9 894 }
KoenKahlman 0:9c82986d7cb9 895 return result;
KoenKahlman 0:9c82986d7cb9 896 }
KoenKahlman 0:9c82986d7cb9 897
KoenKahlman 0:9c82986d7cb9 898 std::list<int> circuit1markers(int sensor_data){
KoenKahlman 0:9c82986d7cb9 899 std::list<int> result;
KoenKahlman 0:9c82986d7cb9 900 sensor_data = ~sensor_data;
KoenKahlman 0:9c82986d7cb9 901 if((sensor_data & 0b00000001) == 0b00000001){
KoenKahlman 0:9c82986d7cb9 902 result.push_back(8);
KoenKahlman 0:9c82986d7cb9 903 }
KoenKahlman 0:9c82986d7cb9 904 if((sensor_data & 0b00000010) == 0b00000010){
KoenKahlman 0:9c82986d7cb9 905 result.push_back(9);
KoenKahlman 0:9c82986d7cb9 906 }
KoenKahlman 0:9c82986d7cb9 907 if((sensor_data & 0b00000100) == 0b00000100){
KoenKahlman 0:9c82986d7cb9 908 result.push_back(10);
KoenKahlman 0:9c82986d7cb9 909 }
KoenKahlman 0:9c82986d7cb9 910 if((sensor_data & 0b00001000) == 0b00001000){
KoenKahlman 0:9c82986d7cb9 911 result.push_back(11);
KoenKahlman 0:9c82986d7cb9 912 }
KoenKahlman 0:9c82986d7cb9 913 if((sensor_data & 0b00010000) == 0b00010000){
KoenKahlman 0:9c82986d7cb9 914 result.push_back(12);
KoenKahlman 0:9c82986d7cb9 915 }
KoenKahlman 0:9c82986d7cb9 916 if((sensor_data & 0b00100000) == 0b00100000){
KoenKahlman 0:9c82986d7cb9 917 result.push_back(13);
KoenKahlman 0:9c82986d7cb9 918 }
KoenKahlman 0:9c82986d7cb9 919 if((sensor_data & 0b01000000) == 0b01000000){
KoenKahlman 1:1648ce825c31 920 //result.push_back(21); do nothing; sensor 21 badly positioned
KoenKahlman 0:9c82986d7cb9 921 }
KoenKahlman 0:9c82986d7cb9 922 if((sensor_data & 0b10000000) == 0b10000000){
KoenKahlman 1:1648ce825c31 923 //result.push_back(22); do nothing; too close
KoenKahlman 0:9c82986d7cb9 924 }
KoenKahlman 0:9c82986d7cb9 925 return result;
KoenKahlman 0:9c82986d7cb9 926 }
KoenKahlman 0:9c82986d7cb9 927
KoenKahlman 0:9c82986d7cb9 928 //LOG OUTPUT
KoenKahlman 0:9c82986d7cb9 929 void displayLCDstatus(){
KoenKahlman 0:9c82986d7cb9 930 lcd.cls();
KoenKahlman 1:1648ce825c31 931 lcd.printf("Train 1: %d \nTrain 2: %d", speed1, speed2);
KoenKahlman 0:9c82986d7cb9 932 }
KoenKahlman 0:9c82986d7cb9 933
KoenKahlman 0:9c82986d7cb9 934 void displayTerminalStatus(){
KoenKahlman 0:9c82986d7cb9 935 string dir1Text = dir1?"forward":"reverse";
KoenKahlman 0:9c82986d7cb9 936 string dir2Text = dir2?"forward":"reverse";
KoenKahlman 0:9c82986d7cb9 937 string light1Text = light1?"on, ":"off,";
KoenKahlman 0:9c82986d7cb9 938 string light2Text = light2?"on, ":"off,";
KoenKahlman 1:1648ce825c31 939 pc.printf("SYSTEM: Iteration %d\n\r", iter);
KoenKahlman 1:1648ce825c31 940 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 941 }