Jasmine Karlsson / Mbed 2 deprecated train_rail

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed TextLCD

main.cpp@30:63a8a5cefc6b, 2018-06-25 (annotated)

Committer:
KwamsC
Date:
Mon Jun 25 14:57:17 2018 +0000
Revision:
30:63a8a5cefc6b
Parent:
29:6031227dcac9
Child:
31:9d973398554f
same section

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