Jasmine Karlsson / Mbed 2 deprecated train_rail

New project

Dependencies:   mbed TextLCD

main.cpp@31:9d973398554f, 2018-06-28 (annotated)

Committer:
jasminealice
Date:
Thu Jun 28 15:08:41 2018 +0000
Revision:
31:9d973398554f
Parent:
30:63a8a5cefc6b 
Fiiinal!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jasminealice 0:57cbecaac770 1 #include "mbed.h"
Kwame 2:9bb410612929 2 #include "TextLCD.h"
jasminealice 18:f5824ba95892 3 #include "MCP23017.h"
jasminealice 18:f5824ba95892 4 #include "Train.h"
jasminealice 20:32ba0a5f2d02 5 #include "Switch.h"
jasminealice 20:32ba0a5f2d02 6 #include "Track.h"
jasminealice 21:31647d80614f 7 #include "Detector.h"
KwamsC 24:418711ed8c52 8 #include <cstdlib>
KwamsC 30:63a8a5cefc6b 9 #include <algorithm>
KwamsC 10:cda57976225f 10
jasminealice 16:55c3c5727f14 11 //Board 1
jasminealice 16:55c3c5727f14 12 /*----------------------------------------------------------------------------
jasminealice 16:55c3c5727f14 13 Pin definitions
jasminealice 16:55c3c5727f14 14 *----------------------------------------------------------------------------*/
jasminealice 17:b7b5f40cebff 15 DigitalOut Track(p20); // train track
jasminealice 17:b7b5f40cebff 16
jasminealice 23:bb57966cb776 17 DigitalOut myled1(LED1), myled2(LED2), myled3(LED3), myled4(LED4);
jasminealice 23:bb57966cb776 18 DigitalOut externalLed1(p15), externalLed2(p16), externalLed3(p17), externalLed4(p18);
KwamsC 14:a5fd98a957e6 19 TextLCD lcd(p22, p21, p23, p24, p25, p26); // lcd
KwamsC 14:a5fd98a957e6 20
jasminealice 23:bb57966cb776 21 DigitalIn sw1(p29), sw2(p30), sw3(p11), sw4(p12);
jasminealice 21:31647d80614f 22
KwamsC 30:63a8a5cefc6b 23 DigitalOut buzz(p10);
KwamsC 30:63a8a5cefc6b 24
jasminealice 23:bb57966cb776 25 InterruptIn int0(p13), int1(p14);
jasminealice 22:c024b20a0e2d 26 I2C i2c(p28, p27);
jasminealice 18:f5824ba95892 27 MCP23017 *mcp;
jasminealice 17:b7b5f40cebff 28
jasminealice 25:90f7a34c253a 29 //DigitalOut en(p6);
jasminealice 25:90f7a34c253a 30
jasminealice 17:b7b5f40cebff 31 /*----------------------------------------------------------------------------
jasminealice 17:b7b5f40cebff 32 Addresses
jasminealice 17:b7b5f40cebff 33 *----------------------------------------------------------------------------*/
jasminealice 21:31647d80614f 34 const unsigned int DCCaddress_silver = 0x01;
jasminealice 21:31647d80614f 35 const unsigned int DCCaddress_red = 0x03;
jasminealice 17:b7b5f40cebff 36 const unsigned int DCCaddress_switch = 0x06;
KwamsC 14:a5fd98a957e6 37
KwamsC 14:a5fd98a957e6 38 /*----------------------------------------------------------------------------
KwamsC 14:a5fd98a957e6 39 Train movement
KwamsC 14:a5fd98a957e6 40 *----------------------------------------------------------------------------*/
KwamsC 14:a5fd98a957e6 41 //move backwards/reverse
KwamsC 14:a5fd98a957e6 42
jasminealice 15:8d96f7a06103 43 //speed dial forward
jasminealice 29:6031227dcac9 44 const unsigned int slow_speed = 0x76; //step 4
jasminealice 18:f5824ba95892 45 //const unsigned int DCCinst_step6 = 0x68; //step 6 1/4 speed
KwamsC 26:f5e71308a13e 46 const unsigned int normal_speed = 0x78; //step 13 1/2 speed
KwamsC 26:f5e71308a13e 47
KwamsC 27:9f9cd0df9a79 48 const unsigned int high_speed = 0x7A; //high speed
KwamsC 27:9f9cd0df9a79 49
jasminealice 17:b7b5f40cebff 50 const unsigned int DCCinst_switch1 = 0x81; //Activate switch1
jasminealice 17:b7b5f40cebff 51 const unsigned int DCCinst_switch2 = 0x82; //Activate switch2
jasminealice 17:b7b5f40cebff 52 const unsigned int DCCinst_switch3 = 0x84; //Activate switch3
jasminealice 17:b7b5f40cebff 53 const unsigned int DCCinst_switch4 = 0x88; //Activate switch4
KwamsC 14:a5fd98a957e6 54 //stop
KwamsC 26:f5e71308a13e 55 const unsigned int stop = 0x40; //forward and stop 01100000
KwamsC 10:cda57976225f 56
jasminealice 25:90f7a34c253a 57 static const int cRight[] = {6,7,8};
jasminealice 25:90f7a34c253a 58 vector<int> C_right (cRight, cRight + sizeof(cRight) / sizeof(cRight[0]) );
jasminealice 25:90f7a34c253a 59
jasminealice 31:9d973398554f 60 static const int cLeft[] = {0,1,2,12,13,14,15};
jasminealice 25:90f7a34c253a 61 vector<int> C_left (cLeft, cLeft + sizeof(cLeft) / sizeof(cLeft[0]) );
jasminealice 25:90f7a34c253a 62
KwamsC 26:f5e71308a13e 63 static const int junction39[] = {3,9};
KwamsC 26:f5e71308a13e 64 vector<int> junction_39 (junction39, junction39 + sizeof(junction39) / sizeof(junction39[0]) );
KwamsC 26:f5e71308a13e 65
KwamsC 26:f5e71308a13e 66 static const int junction511[] = {5,11};
KwamsC 26:f5e71308a13e 67 vector<int> junction_511 (junction511, junction511 + sizeof(junction511) / sizeof(junction511[0]) );
KwamsC 26:f5e71308a13e 68
jasminealice 29:6031227dcac9 69 static const int lane4[] = {4};
jasminealice 29:6031227dcac9 70 vector<int> lane_4 (lane4, lane4 + sizeof(lane4) / sizeof(lane4[0]) );
jasminealice 29:6031227dcac9 71
jasminealice 29:6031227dcac9 72 static const int lane10[] = {10};
jasminealice 29:6031227dcac9 73 vector<int> lane_10 (lane10, lane10 + sizeof(lane10) / sizeof(lane10[0]) );
jasminealice 29:6031227dcac9 74
jasminealice 28:1430685f4d12 75 int redStartVal = 10;
jasminealice 29:6031227dcac9 76 int silverStartVal = 15;
KwamsC 26:f5e71308a13e 77
jasminealice 17:b7b5f40cebff 78 /*----------------------------------------------------------------------------
jasminealice 17:b7b5f40cebff 79 Function definitions
jasminealice 17:b7b5f40cebff 80 *----------------------------------------------------------------------------*/
jasminealice 17:b7b5f40cebff 81 bool readSwitch(DigitalIn theSwitch);
KwamsC 30:63a8a5cefc6b 82 bool sameSection();
jasminealice 23:bb57966cb776 83 int convertHextoDec(int pos, int intnr);
KwamsC 24:418711ed8c52 84 void checkDetector(int inter);
jasminealice 22:c024b20a0e2d 85 void init_mcp();
jasminealice 23:bb57966cb776 86 void init_interrupt();
KwamsC 24:418711ed8c52 87 void init_trains();
jasminealice 31:9d973398554f 88 void endFunction();
KwamsC 27:9f9cd0df9a79 89 void executeCase(int pos, Train* currTrain);
jasminealice 23:bb57966cb776 90 Train& assignTrain(int pos);
KwamsC 10:cda57976225f 91
jasminealice 21:31647d80614f 92 //Trains
jasminealice 28:1430685f4d12 93 Train redTrain(DCCaddress_red, normal_speed, redStartVal, high_speed, normal_speed);
jasminealice 28:1430685f4d12 94 Train silverTrain(DCCaddress_silver,normal_speed, silverStartVal, normal_speed, slow_speed);
jasminealice 21:31647d80614f 95 //Switches
jasminealice 21:31647d80614f 96 Switch switch1(DCCaddress_switch,DCCinst_switch1);
KwamsC 24:418711ed8c52 97 Switch switch2(DCCaddress_switch,DCCinst_switch2);
jasminealice 21:31647d80614f 98 Switch switch3(DCCaddress_switch,DCCinst_switch3);
jasminealice 21:31647d80614f 99 Switch switch4(DCCaddress_switch,DCCinst_switch4);
jasminealice 21:31647d80614f 100 //Detectors
jasminealice 23:bb57966cb776 101 Detector dect(false);
jasminealice 21:31647d80614f 102
jasminealice 16:55c3c5727f14 103 /*----------------------------------------------------------------------------
jasminealice 16:55c3c5727f14 104 Main
jasminealice 16:55c3c5727f14 105 *----------------------------------------------------------------------------*/
jasminealice 16:55c3c5727f14 106 int main() {
KwamsC 30:63a8a5cefc6b 107 buzz = 0;
jasminealice 22:c024b20a0e2d 108 lcd.printf("Start journey");
jasminealice 22:c024b20a0e2d 109 init_mcp();
jasminealice 23:bb57966cb776 110 init_interrupt();
KwamsC 24:418711ed8c52 111 init_trains();
jasminealice 21:31647d80614f 112 while(1){
jasminealice 23:bb57966cb776 113 redTrain.sendCommand();
jasminealice 23:bb57966cb776 114 silverTrain.sendCommand();
jasminealice 22:c024b20a0e2d 115
jasminealice 21:31647d80614f 116 if(!readSwitch(sw3)){ //Change speed
jasminealice 28:1430685f4d12 117 lcd.cls();
jasminealice 23:bb57966cb776 118 lcd.printf("Go forward");
jasminealice 28:1430685f4d12 119 redTrain.changeSpeed(high_speed);
KwamsC 26:f5e71308a13e 120 silverTrain.changeSpeed(normal_speed);
jasminealice 22:c024b20a0e2d 121 }
jasminealice 23:bb57966cb776 122
KwamsC 24:418711ed8c52 123 if(redTrain.checkStop()){ //check if Train stopped
jasminealice 28:1430685f4d12 124 lcd.cls();
jasminealice 28:1430685f4d12 125 lcd.printf("%d", redTrain.getPosition());
KwamsC 27:9f9cd0df9a79 126 executeCase(redTrain.getPosition(), &redTrain);
KwamsC 24:418711ed8c52 127 }
KwamsC 24:418711ed8c52 128 else if(silverTrain.checkStop()){
KwamsC 27:9f9cd0df9a79 129 executeCase(silverTrain.getPosition(), &silverTrain);
jasminealice 25:90f7a34c253a 130 }
jasminealice 25:90f7a34c253a 131
KwamsC 30:63a8a5cefc6b 132 if (sameSection()){
KwamsC 30:63a8a5cefc6b 133 lcd.cls();
KwamsC 30:63a8a5cefc6b 134 lcd.printf("SAME SEC");
KwamsC 30:63a8a5cefc6b 135 silverTrain.Stop();
KwamsC 30:63a8a5cefc6b 136 redTrain.Stop();
KwamsC 30:63a8a5cefc6b 137 }
KwamsC 30:63a8a5cefc6b 138
jasminealice 25:90f7a34c253a 139 if(redTrain.checkStop() && silverTrain.checkStop()) {
jasminealice 28:1430685f4d12 140 lcd.cls();
jasminealice 28:1430685f4d12 141 lcd.printf("Stop loop...");
jasminealice 25:90f7a34c253a 142 break;
KwamsC 30:63a8a5cefc6b 143 }
jasminealice 22:c024b20a0e2d 144 }
jasminealice 25:90f7a34c253a 145
jasminealice 31:9d973398554f 146 endFunction();
jasminealice 25:90f7a34c253a 147 return 0;
jasminealice 16:55c3c5727f14 148 }
KwamsC 10:cda57976225f 149
KwamsC 14:a5fd98a957e6 150 /*----------------------------------------------------------------------------
KwamsC 14:a5fd98a957e6 151 Functions
KwamsC 14:a5fd98a957e6 152 *----------------------------------------------------------------------------*/
jasminealice 23:bb57966cb776 153 int convertHextoDec(int pos, int intnr){
jasminealice 23:bb57966cb776 154 int newPos;
jasminealice 23:bb57966cb776 155 switch(pos){
jasminealice 23:bb57966cb776 156 case 0xfe:
jasminealice 23:bb57966cb776 157 newPos = 0;
jasminealice 23:bb57966cb776 158 break;
jasminealice 23:bb57966cb776 159 case 0xfd:
jasminealice 23:bb57966cb776 160 newPos = 1;
jasminealice 23:bb57966cb776 161 break;
jasminealice 23:bb57966cb776 162 case 0xfb:
jasminealice 23:bb57966cb776 163 newPos = 2;
jasminealice 23:bb57966cb776 164 break;
jasminealice 23:bb57966cb776 165 case 0xf7:
jasminealice 23:bb57966cb776 166 newPos = 3;
jasminealice 23:bb57966cb776 167 break;
jasminealice 23:bb57966cb776 168 case 0xef:
jasminealice 23:bb57966cb776 169 newPos = 4;
jasminealice 23:bb57966cb776 170 break;
jasminealice 23:bb57966cb776 171 case 0xdf:
jasminealice 23:bb57966cb776 172 newPos = 5;
jasminealice 23:bb57966cb776 173 break;
jasminealice 23:bb57966cb776 174 case 0xbf:
jasminealice 23:bb57966cb776 175 newPos = 6;
jasminealice 23:bb57966cb776 176 break;
jasminealice 23:bb57966cb776 177 case 0x7f:
jasminealice 23:bb57966cb776 178 newPos = 7;
jasminealice 23:bb57966cb776 179 break;
jasminealice 23:bb57966cb776 180 default:
jasminealice 23:bb57966cb776 181 return -1;
jasminealice 23:bb57966cb776 182 }
jasminealice 23:bb57966cb776 183
jasminealice 23:bb57966cb776 184 if(intnr == 1)
jasminealice 23:bb57966cb776 185 newPos += 8;
jasminealice 23:bb57966cb776 186 return newPos;
jasminealice 23:bb57966cb776 187 }
jasminealice 21:31647d80614f 188
jasminealice 31:9d973398554f 189 void endFunction(){
jasminealice 31:9d973398554f 190
jasminealice 31:9d973398554f 191 lcd.printf("Shutting down...");
jasminealice 31:9d973398554f 192 bool in = true;
jasminealice 31:9d973398554f 193 while(in){
jasminealice 31:9d973398554f 194 redTrain.sendCommand();
jasminealice 31:9d973398554f 195 silverTrain.sendCommand();
jasminealice 31:9d973398554f 196 externalLed2 = 1;
jasminealice 31:9d973398554f 197 // wait(0.2);
jasminealice 31:9d973398554f 198 //externalLed2 = 0;
jasminealice 31:9d973398554f 199 buzz = 1;
jasminealice 31:9d973398554f 200 if(!readSwitch(sw4)){
jasminealice 31:9d973398554f 201 in = false;
jasminealice 31:9d973398554f 202 }
jasminealice 31:9d973398554f 203
jasminealice 31:9d973398554f 204 }
jasminealice 31:9d973398554f 205 externalLed2 = 0;
jasminealice 31:9d973398554f 206 buzz = 0;
jasminealice 31:9d973398554f 207 }
jasminealice 31:9d973398554f 208
jasminealice 23:bb57966cb776 209 Train& assignTrain(int pos){
jasminealice 23:bb57966cb776 210
jasminealice 21:31647d80614f 211 //Check which train got the interupt
KwamsC 27:9f9cd0df9a79 212 if(redTrain.checkInterupt(pos) && !redTrain.checkStop()){
jasminealice 21:31647d80614f 213 redTrain.setPosition(pos);
jasminealice 23:bb57966cb776 214 return redTrain;
jasminealice 21:31647d80614f 215 }
KwamsC 27:9f9cd0df9a79 216 else if(silverTrain.checkInterupt(pos) && !silverTrain.checkStop()){
jasminealice 21:31647d80614f 217 silverTrain.setPosition(pos);
jasminealice 23:bb57966cb776 218 return silverTrain;
jasminealice 28:1430685f4d12 219 }else if(redTrain.checkInterupt(pos) && redTrain.checkStop()){
jasminealice 28:1430685f4d12 220 return redTrain;
jasminealice 28:1430685f4d12 221 }else if(silverTrain.checkInterupt(pos) && silverTrain.checkStop()){
jasminealice 28:1430685f4d12 222 return silverTrain;
jasminealice 28:1430685f4d12 223 }
jasminealice 28:1430685f4d12 224 else{
KwamsC 30:63a8a5cefc6b 225 lcd.cls();
KwamsC 30:63a8a5cefc6b 226 lcd.printf("NO TRAIN ASSIGNED");
KwamsC 30:63a8a5cefc6b 227 lcd.printf("%d", pos);
jasminealice 31:9d973398554f 228
KwamsC 30:63a8a5cefc6b 229 silverTrain.Stop();
KwamsC 30:63a8a5cefc6b 230 redTrain.Stop();
jasminealice 31:9d973398554f 231 endFunction();
jasminealice 21:31647d80614f 232 }
jasminealice 23:bb57966cb776 233 }
jasminealice 23:bb57966cb776 234
KwamsC 27:9f9cd0df9a79 235 void executeCase(int pos, Train* currTrain){
jasminealice 28:1430685f4d12 236 lcd.cls();
jasminealice 31:9d973398554f 237 lcd.printf("At ");
jasminealice 31:9d973398554f 238 lcd.printf("%d", pos);
KwamsC 24:418711ed8c52 239 switch(pos){
jasminealice 25:90f7a34c253a 240 case 0:{
KwamsC 27:9f9cd0df9a79 241 currTrain->changeSpeed(currTrain->normalSpeed());
jasminealice 25:90f7a34c253a 242 break;}
jasminealice 25:90f7a34c253a 243 case 1:{
jasminealice 25:90f7a34c253a 244 //TO DO: Check which train is going and only slow down if it started from 2
jasminealice 25:90f7a34c253a 245 /*if(!currTrain->isClockwise()){
KwamsC 26:f5e71308a13e 246 currTrain->changeSpeed(slow_speed);//Slow down
jasminealice 25:90f7a34c253a 247 lcd.printf("slow down");
jasminealice 25:90f7a34c253a 248 }*/
jasminealice 25:90f7a34c253a 249 //else
jasminealice 31:9d973398554f 250 currTrain->changeSpeed(currTrain->slowlySpeed());
jasminealice 25:90f7a34c253a 251
jasminealice 25:90f7a34c253a 252 break;}
jasminealice 25:90f7a34c253a 253 case 2:{
KwamsC 24:418711ed8c52 254 /*
jasminealice 25:90f7a34c253a 255 Stop everytime at 2
KwamsC 24:418711ed8c52 256 */
KwamsC 30:63a8a5cefc6b 257 dect.showReservation();
jasminealice 29:6031227dcac9 258 if(!dect.checkReservation(10) && !dect.checkReservation(11)){
jasminealice 28:1430685f4d12 259 currTrain->Stop();
jasminealice 28:1430685f4d12 260 lcd.printf("Stopping at 2");
jasminealice 28:1430685f4d12 261 }
jasminealice 29:6031227dcac9 262 else{
jasminealice 28:1430685f4d12 263
jasminealice 28:1430685f4d12 264 if(!dect.checkReservation(4) && !dect.checkReservation(3)){ //Check if 4 or 3 is reserved
jasminealice 28:1430685f4d12 265 int v1 = rand() % 100; //IF NOT randomize which way to go
jasminealice 28:1430685f4d12 266 if (v1 < 50){ //0.25 chance to enable the switch2, reserve 3,9
jasminealice 28:1430685f4d12 267 switch2.switchOn();
jasminealice 28:1430685f4d12 268 dect.makeReservation(junction_39);
jasminealice 28:1430685f4d12 269 }
jasminealice 28:1430685f4d12 270 else { //0.75 chance to disable switch2, reserve 4
jasminealice 28:1430685f4d12 271 switch2.switchOff();
jasminealice 29:6031227dcac9 272 dect.makeReservation(lane_4);
jasminealice 28:1430685f4d12 273 }
jasminealice 28:1430685f4d12 274 }else if(dect.checkReservation(4)){ //IF 4 is reserved
jasminealice 28:1430685f4d12 275 switch2.switchOn(); //Enable switch2, reserve 3,9
KwamsC 26:f5e71308a13e 276 dect.makeReservation(junction_39);
jasminealice 28:1430685f4d12 277
jasminealice 28:1430685f4d12 278 }else{ //IF 3 reserved
jasminealice 28:1430685f4d12 279 switch2.switchOff(); //Disable switch2, reserve 4
jasminealice 29:6031227dcac9 280 dect.makeReservation(lane_4);
jasminealice 28:1430685f4d12 281 }
jasminealice 28:1430685f4d12 282 currTrain->changeSpeed(currTrain->normalSpeed()); //Go forward
jasminealice 29:6031227dcac9 283 }
jasminealice 25:90f7a34c253a 284 break;}
jasminealice 31:9d973398554f 285 case 3:{
jasminealice 25:90f7a34c253a 286 switch2.switchOff(); //Disable switch2
jasminealice 28:1430685f4d12 287 dect.clearReservation(C_left); //free nr 0,1,2,12,13
jasminealice 28:1430685f4d12 288 if(!dect.checkReservation(5)){
KwamsC 30:63a8a5cefc6b 289 currTrain->changeDirection(); //change direction
jasminealice 31:9d973398554f 290 currTrain->changeSpeed(currTrain->slowlySpeed()); //go forward
jasminealice 28:1430685f4d12 291 }
jasminealice 28:1430685f4d12 292 else{
jasminealice 28:1430685f4d12 293 currTrain->Stop();
jasminealice 28:1430685f4d12 294 }
jasminealice 29:6031227dcac9 295 break;
jasminealice 29:6031227dcac9 296 }
jasminealice 25:90f7a34c253a 297 case 4:{
jasminealice 25:90f7a34c253a 298 dect.clearReservation(C_left); //free nr 0,1,2,12,13
jasminealice 25:90f7a34c253a 299 switch3.switchOn(); //Turn on switch3
jasminealice 28:1430685f4d12 300 dect.showReservation();
jasminealice 31:9d973398554f 301 if(currTrain->isClockwise()){
jasminealice 31:9d973398554f 302 lcd.printf("Switch 3 not activated");
jasminealice 31:9d973398554f 303 redTrain.Stop();
jasminealice 31:9d973398554f 304 silverTrain.Stop();
jasminealice 31:9d973398554f 305 }
jasminealice 31:9d973398554f 306 else if(dect.checkReservation(6)){ //Check if 6 is reserved
jasminealice 25:90f7a34c253a 307 currTrain->Stop(); //IF yes STOP
KwamsC 24:418711ed8c52 308 }
jasminealice 25:90f7a34c253a 309 else{
jasminealice 25:90f7a34c253a 310 dect.makeReservation(C_right); //else reserve 6,7,8 and go forward
KwamsC 27:9f9cd0df9a79 311 currTrain->changeSpeed(currTrain->normalSpeed());
KwamsC 24:418711ed8c52 312 }
jasminealice 29:6031227dcac9 313 break;
jasminealice 29:6031227dcac9 314 }
jasminealice 25:90f7a34c253a 315 case 5:{
jasminealice 31:9d973398554f 316 dect.makeReservation(junction_511);
jasminealice 25:90f7a34c253a 317 switch3.switchOn(); //Enable switch3
jasminealice 25:90f7a34c253a 318 dect.clearReservation(C_right); //free nr 6,7,8
jasminealice 28:1430685f4d12 319
KwamsC 30:63a8a5cefc6b 320 if(!dect.checkReservation(3)){
KwamsC 30:63a8a5cefc6b 321 currTrain->changeDirection(); //change direction
KwamsC 30:63a8a5cefc6b 322 currTrain->changeSpeed(currTrain->slowlySpeed()); //go forward
jasminealice 28:1430685f4d12 323 }
jasminealice 28:1430685f4d12 324 else{
jasminealice 28:1430685f4d12 325 currTrain->Stop();
KwamsC 30:63a8a5cefc6b 326 }
jasminealice 29:6031227dcac9 327 break;
jasminealice 29:6031227dcac9 328 }
jasminealice 25:90f7a34c253a 329 case 6:{
KwamsC 24:418711ed8c52 330 if(!currTrain->isClockwise()) //IF CC goes towards 7
KwamsC 24:418711ed8c52 331 {
KwamsC 24:418711ed8c52 332 switch3.switchOn(); //Enable switch3
jasminealice 29:6031227dcac9 333 //int arr [3] = {4,5,11};
jasminealice 29:6031227dcac9 334 //vector<int> detectors(arr, arr + sizeof(arr) / sizeof(int));
jasminealice 29:6031227dcac9 335 dect.clearReservation(lane_4);
jasminealice 29:6031227dcac9 336 dect.clearReservation(junction_511); //free nr 4,5,11
KwamsC 27:9f9cd0df9a79 337 currTrain->changeSpeed(currTrain->normalSpeed()); //Move forward
KwamsC 24:418711ed8c52 338 }
KwamsC 24:418711ed8c52 339 else{
jasminealice 25:90f7a34c253a 340 if(dect.checkReservation(5)){ //IF 5 is reserved
jasminealice 25:90f7a34c253a 341 currTrain->Stop();
jasminealice 25:90f7a34c253a 342
jasminealice 25:90f7a34c253a 343 }else{ //else 5 reserved
jasminealice 25:90f7a34c253a 344 switch3.switchOff(); //Enable switch3, reserve 4
KwamsC 26:f5e71308a13e 345 dect.makeReservation(junction_511);
KwamsC 27:9f9cd0df9a79 346 currTrain->changeSpeed(currTrain->normalSpeed());
KwamsC 24:418711ed8c52 347 }
KwamsC 24:418711ed8c52 348 }
jasminealice 29:6031227dcac9 349 break;
jasminealice 29:6031227dcac9 350 }
jasminealice 31:9d973398554f 351 case 7:{
jasminealice 31:9d973398554f 352 currTrain->changeSpeed(currTrain->slowlySpeed());
jasminealice 25:90f7a34c253a 353 break;}
jasminealice 31:9d973398554f 354 case 8:{
KwamsC 24:418711ed8c52 355 if(currTrain->isClockwise()) //IF C goes towards 7
KwamsC 24:418711ed8c52 356 {
KwamsC 24:418711ed8c52 357 switch4.switchOff(); //Disable switch4
KwamsC 26:f5e71308a13e 358 dect.clearReservation(junction_39); //Free 3,9,10
KwamsC 27:9f9cd0df9a79 359 currTrain->changeSpeed(currTrain->normalSpeed()); //Move forward
KwamsC 24:418711ed8c52 360 }
jasminealice 28:1430685f4d12 361 else{ //IF CC if 10 is reserved stop
jasminealice 28:1430685f4d12 362 if(dect.checkReservation(10)){
jasminealice 28:1430685f4d12 363 currTrain->Stop();
jasminealice 28:1430685f4d12 364 }else{
jasminealice 28:1430685f4d12 365 switch4.switchOff(); //Disable switch4, reserve 10
jasminealice 29:6031227dcac9 366 dect.makeReservation(lane_10);
jasminealice 28:1430685f4d12 367 currTrain->changeSpeed(currTrain->slowlySpeed()); //Go forward
jasminealice 28:1430685f4d12 368 }
KwamsC 24:418711ed8c52 369 }
jasminealice 29:6031227dcac9 370 break;
jasminealice 29:6031227dcac9 371 }
jasminealice 31:9d973398554f 372 case 9:{
jasminealice 31:9d973398554f 373 if(!currTrain->isClockwise()){
jasminealice 31:9d973398554f 374 lcd.printf("Switch 4 is activated");
jasminealice 31:9d973398554f 375 redTrain.Stop();
jasminealice 31:9d973398554f 376 silverTrain.Stop();
jasminealice 31:9d973398554f 377 }
jasminealice 31:9d973398554f 378 else if(!dect.checkReservation(8)){
jasminealice 25:90f7a34c253a 379 switch4.switchOn(); //Enable switch4
jasminealice 25:90f7a34c253a 380 dect.makeReservation(C_right); //Reserve 6,7,8
KwamsC 27:9f9cd0df9a79 381 currTrain->changeSpeed(currTrain->normalSpeed()); //Go forward
KwamsC 24:418711ed8c52 382
KwamsC 24:418711ed8c52 383 }
jasminealice 25:90f7a34c253a 384 else{
jasminealice 28:1430685f4d12 385 lcd.printf("8 is reserved");
jasminealice 25:90f7a34c253a 386 currTrain->Stop();
jasminealice 25:90f7a34c253a 387 }
jasminealice 29:6031227dcac9 388 break;
jasminealice 29:6031227dcac9 389 }
jasminealice 31:9d973398554f 390 case 10:{
jasminealice 31:9d973398554f 391 dect.makeReservation(lane_10);
jasminealice 25:90f7a34c253a 392 dect.clearReservation(C_right); //free nr 6, 7, 8, 9
jasminealice 25:90f7a34c253a 393 if(dect.checkReservation(12)){ //Check if 12 is reserved
jasminealice 25:90f7a34c253a 394 currTrain->Stop(); //IF yes STOP
KwamsC 24:418711ed8c52 395 }
jasminealice 28:1430685f4d12 396 else{
jasminealice 28:1430685f4d12 397 switch1.switchOff(); //Turn off switch1
jasminealice 25:90f7a34c253a 398 dect.makeReservation(C_left);
KwamsC 27:9f9cd0df9a79 399 currTrain->changeSpeed(currTrain->normalSpeed()); //go forward
jasminealice 25:90f7a34c253a 400 }
jasminealice 29:6031227dcac9 401 break;
jasminealice 29:6031227dcac9 402 }
jasminealice 25:90f7a34c253a 403
jasminealice 31:9d973398554f 404 case 11:{
jasminealice 31:9d973398554f 405 dect.makeReservation(junction_511);
jasminealice 31:9d973398554f 406 dect.showReservation();
jasminealice 25:90f7a34c253a 407 if(!dect.checkReservation(12)){
jasminealice 25:90f7a34c253a 408
jasminealice 25:90f7a34c253a 409 switch1.switchOn(); //Enable switch1
jasminealice 25:90f7a34c253a 410 dect.makeReservation(C_left); //Reserve 0,1,2, 12, 13
KwamsC 27:9f9cd0df9a79 411 currTrain->changeSpeed(currTrain->normalSpeed()); //go forward
jasminealice 25:90f7a34c253a 412 }
jasminealice 25:90f7a34c253a 413 else{
jasminealice 28:1430685f4d12 414 currTrain->Stop();
jasminealice 25:90f7a34c253a 415 }
KwamsC 24:418711ed8c52 416 break;
jasminealice 29:6031227dcac9 417 }
jasminealice 31:9d973398554f 418 case 12: {
jasminealice 25:90f7a34c253a 419 switch1.switchOff(); //Turn of switch1
jasminealice 29:6031227dcac9 420 //int arr2 [3] = {5,10,11};
jasminealice 29:6031227dcac9 421 //vector<int> detectors2(arr2, arr2 + sizeof(arr2) / sizeof(int));
jasminealice 29:6031227dcac9 422 dect.clearReservation(lane_10); //free nr 5, 10, 11
jasminealice 29:6031227dcac9 423 dect.clearReservation(junction_511);
jasminealice 31:9d973398554f 424 dect.makeReservation(C_left);
KwamsC 27:9f9cd0df9a79 425 currTrain->changeSpeed(currTrain->normalSpeed()); //Move forward
jasminealice 25:90f7a34c253a 426
jasminealice 29:6031227dcac9 427 break;
jasminealice 29:6031227dcac9 428 }
jasminealice 25:90f7a34c253a 429 case 13: {
KwamsC 27:9f9cd0df9a79 430 currTrain->changeSpeed(currTrain->normalSpeed());
jasminealice 29:6031227dcac9 431 break;
jasminealice 29:6031227dcac9 432 }
jasminealice 31:9d973398554f 433 case 14:{
jasminealice 28:1430685f4d12 434 currTrain->changeSpeed(currTrain->slowlySpeed());
jasminealice 29:6031227dcac9 435 break;
jasminealice 29:6031227dcac9 436 }
jasminealice 25:90f7a34c253a 437 case 15:{
jasminealice 31:9d973398554f 438 currTrain->changeSpeed(currTrain->slowlySpeed());
jasminealice 29:6031227dcac9 439 break;
jasminealice 29:6031227dcac9 440 }
jasminealice 25:90f7a34c253a 441 default: {
KwamsC 24:418711ed8c52 442 lcd.printf("Not in a valid case");
KwamsC 26:f5e71308a13e 443 currTrain->Stop();
jasminealice 29:6031227dcac9 444 break;
jasminealice 29:6031227dcac9 445 }
KwamsC 24:418711ed8c52 446 }
KwamsC 24:418711ed8c52 447
KwamsC 24:418711ed8c52 448 mcp->_read(GPIOA);
KwamsC 24:418711ed8c52 449 mcp->_read(GPIOB);
KwamsC 24:418711ed8c52 450 }
KwamsC 24:418711ed8c52 451
KwamsC 24:418711ed8c52 452 void checkDetector(int inter){
jasminealice 23:bb57966cb776 453 lcd.cls();
jasminealice 23:bb57966cb776 454 int pos;
jasminealice 23:bb57966cb776 455 wait_us(100);
jasminealice 23:bb57966cb776 456 if (inter == 0){
jasminealice 23:bb57966cb776 457 //Read from first one
jasminealice 23:bb57966cb776 458 pos = mcp->_read(INTCAPA);
jasminealice 23:bb57966cb776 459 pos = convertHextoDec(pos, 0);
jasminealice 21:31647d80614f 460 }
jasminealice 23:bb57966cb776 461 else if (inter == 1){
jasminealice 23:bb57966cb776 462 //Read from second one
jasminealice 23:bb57966cb776 463 pos = mcp->_read(INTCAPB);
jasminealice 23:bb57966cb776 464 pos = convertHextoDec(pos, 1);
jasminealice 23:bb57966cb776 465 }
jasminealice 31:9d973398554f 466 else{
jasminealice 23:bb57966cb776 467 //PROBLEM
jasminealice 23:bb57966cb776 468 lcd.printf("Fail to detect int");
jasminealice 31:9d973398554f 469 return;
jasminealice 28:1430685f4d12 470 }
jasminealice 31:9d973398554f 471
jasminealice 31:9d973398554f 472 Train* currTrain = &assignTrain(pos);
jasminealice 31:9d973398554f 473 executeCase(pos, currTrain);
jasminealice 31:9d973398554f 474
jasminealice 15:8d96f7a06103 475 }
jasminealice 15:8d96f7a06103 476
jasminealice 17:b7b5f40cebff 477 bool readSwitch(DigitalIn theSwitch){
jasminealice 17:b7b5f40cebff 478 int val = theSwitch.read();
jasminealice 16:55c3c5727f14 479 if(val == 1)
jasminealice 29:6031227dcac9 480 return true;
jasminealice 16:55c3c5727f14 481 else
jasminealice 29:6031227dcac9 482 return false;
tanjinamicky 13:78a2481fd65b 483 }
tanjinamicky 13:78a2481fd65b 484
jasminealice 22:c024b20a0e2d 485 void init_mcp() {
jasminealice 22:c024b20a0e2d 486 // Initialisation of MCP registers,documentation on registers is availableatNiels/Jacco/Natalia
jasminealice 22:c024b20a0e2d 487 mcp = new MCP23017(i2c, 0x40);
jasminealice 22:c024b20a0e2d 488 mcp->_write(IODIRA, (unsigned char )0xff);
jasminealice 22:c024b20a0e2d 489 mcp->_write(IODIRB, (unsigned char )0xff);
jasminealice 22:c024b20a0e2d 490 mcp->_write(IPOLA, (unsigned char )0x00);
jasminealice 22:c024b20a0e2d 491 mcp->_write(IPOLB, (unsigned char )0x00);
jasminealice 22:c024b20a0e2d 492 mcp->_write(DEFVALA, (unsigned char )0xff);
jasminealice 22:c024b20a0e2d 493 mcp->_write(DEFVALB, (unsigned char )0xff);
jasminealice 22:c024b20a0e2d 494 mcp->_write(INTCONA, (unsigned char )0xff);
jasminealice 22:c024b20a0e2d 495 mcp->_write(INTCONB, (unsigned char )0xff);
jasminealice 22:c024b20a0e2d 496 mcp->_write(IOCONA, (unsigned char )0x2);
jasminealice 22:c024b20a0e2d 497 mcp->_write(IOCONB, (unsigned char )0x2);
jasminealice 22:c024b20a0e2d 498 mcp->_write(GPPUA, (unsigned char )0xff);
jasminealice 22:c024b20a0e2d 499 mcp->_write(GPPUB, (unsigned char )0xff);
jasminealice 18:f5824ba95892 500 }
jasminealice 23:bb57966cb776 501 void passZero(){
jasminealice 23:bb57966cb776 502 checkDetector(0);
jasminealice 23:bb57966cb776 503 }
jasminealice 22:c024b20a0e2d 504
jasminealice 23:bb57966cb776 505 void passOne(){
jasminealice 23:bb57966cb776 506 checkDetector(1);
jasminealice 23:bb57966cb776 507 }
jasminealice 23:bb57966cb776 508
jasminealice 23:bb57966cb776 509 void init_interrupt() {
jasminealice 22:c024b20a0e2d 510 // Clear current interrupts
jasminealice 25:90f7a34c253a 511 //en = 1;
jasminealice 22:c024b20a0e2d 512 mcp->_read(GPIOA);
jasminealice 22:c024b20a0e2d 513 mcp->_read(GPIOB);
jasminealice 22:c024b20a0e2d 514 // Register callbacks
jasminealice 23:bb57966cb776 515 int0.fall(&passZero);
jasminealice 23:bb57966cb776 516 int1.fall(&passOne);
jasminealice 22:c024b20a0e2d 517 // Enable interrupts on MCP
jasminealice 22:c024b20a0e2d 518 mcp->_write(GPINTENA, (unsigned char )0xff);
jasminealice 22:c024b20a0e2d 519 mcp->_write(GPINTENB, (unsigned char )0xff);// Ready to go!
jasminealice 22:c024b20a0e2d 520 }
KwamsC 24:418711ed8c52 521
KwamsC 30:63a8a5cefc6b 522 bool sameSection(){
KwamsC 30:63a8a5cefc6b 523 int posRed = redTrain.getPosition();
KwamsC 30:63a8a5cefc6b 524 int posSilver = silverTrain.getPosition();
KwamsC 30:63a8a5cefc6b 525
KwamsC 30:63a8a5cefc6b 526 if ((std::find(C_left.begin(), C_left.end(), posRed) !=C_left.end()) && (std::find(C_left.begin(), C_left.end(), posSilver) !=C_left.end()))
KwamsC 30:63a8a5cefc6b 527 return true;
KwamsC 30:63a8a5cefc6b 528 else if ((std::find(C_right.begin(), C_right.end(), posRed) !=C_right.end()) && (std::find(C_right.begin(), C_right.end(), posSilver) !=C_right.end()))
KwamsC 30:63a8a5cefc6b 529 return true;
KwamsC 30:63a8a5cefc6b 530 else if ((std::find(junction_511.begin(), junction_511.end(), posRed) !=junction_511.end()) && (std::find(junction_511.begin(), junction_511.end(), posSilver) !=junction_511.end()))
KwamsC 30:63a8a5cefc6b 531 return true;
KwamsC 30:63a8a5cefc6b 532 else if ((std::find(junction_39.begin(), junction_39.end(), posRed) !=junction_39.end()) && (std::find(junction_39.begin(), junction_39.end(), posSilver) !=junction_39.end()))
KwamsC 30:63a8a5cefc6b 533 return true;
KwamsC 30:63a8a5cefc6b 534
KwamsC 30:63a8a5cefc6b 535 return false;
KwamsC 30:63a8a5cefc6b 536 }
KwamsC 30:63a8a5cefc6b 537
KwamsC 24:418711ed8c52 538 void init_trains(){
KwamsC 26:f5e71308a13e 539 redTrain.changeSpeed(stop);
KwamsC 26:f5e71308a13e 540 silverTrain.changeSpeed(stop);
KwamsC 24:418711ed8c52 541
KwamsC 24:418711ed8c52 542 vector<int> res;
KwamsC 24:418711ed8c52 543 res.push_back(2);
jasminealice 25:90f7a34c253a 544 //res.push_back(4);
jasminealice 28:1430685f4d12 545 dect.makeReservation(C_left); //Run trains counterclockwise and reserve
KwamsC 24:418711ed8c52 546 vector<int> res2;
jasminealice 28:1430685f4d12 547 res2.push_back(10);
jasminealice 25:90f7a34c253a 548 //res2.push_back(13);
KwamsC 24:418711ed8c52 549 dect.makeReservation(res2);
jasminealice 28:1430685f4d12 550 dect.showReservation();
jasminealice 25:90f7a34c253a 551 }