Denver / Mbed 2 deprecated denver_train_proj

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@55:aa3baa01f43d, 2018-06-21 (annotated)

Committer:
mglmx
Date:
Thu Jun 21 12:19:29 2018 +0000
Revision:
55:aa3baa01f43d
Parent:
52:c08495446f87
Child:
56:fe999806787d
Both trains are detected. Apparently NAC is detected.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mglmx 0:4d06a6a8e785 1 #include "mbed.h"
mglmx 3:fe7010b693a0 2 #include "TextLCD.h"
mglmx 18:aa43bb62e60f 3 #include "MCP23017.h"
mglmx 22:e4153ca757dd 4 #include <string>
mglmx 22:e4153ca757dd 5 #include <iostream>
mglmx 35:cfcfeccb959e 6 #include <vector>
mglmx 35:cfcfeccb959e 7
mglmx 35:cfcfeccb959e 8 using namespace std;
mglmx 18:aa43bb62e60f 9
mglmx 1:0ab26889af9b 10
carlosperales95 7:e2b8461d4f05 11 /******PINS AND DECLARATIONS*******/
carlosperales95 7:e2b8461d4f05 12
carlosperales95 25:a42a1ed4d8e9 13 //------PINS
mglmx 21:e6f1649add39 14
mglmx 21:e6f1649add39 15 //SWITCHES p5 - p8
mglmx 21:e6f1649add39 16 DigitalIn switch1(p5);
mglmx 21:e6f1649add39 17 DigitalIn switch2(p6);
mglmx 21:e6f1649add39 18 DigitalIn switch3(p7);
mglmx 21:e6f1649add39 19 DigitalIn switch4(p8);
carlosperales95 7:e2b8461d4f05 20
carlosperales95 10:2088b1935a93 21 //RAIL SENSORS - INT0,INT1
carlosperales95 7:e2b8461d4f05 22 //INT0 - p9
mglmx 18:aa43bb62e60f 23 InterruptIn int0(p9);
carlosperales95 7:e2b8461d4f05 24 //INT1 - p10
mglmx 18:aa43bb62e60f 25 InterruptIn int1(p10);
carlosperales95 7:e2b8461d4f05 26
carlosperales95 7:e2b8461d4f05 27 ///p11
carlosperales95 7:e2b8461d4f05 28 ///p12
carlosperales95 12:e914ca5cd44b 29
carlosperales95 7:e2b8461d4f05 30 //M0 - p13
mglmx 35:cfcfeccb959e 31 DigitalIn d21stat(p13); //Sensor right of the station
carlosperales95 7:e2b8461d4f05 32 //M1 - p14
mglmx 35:cfcfeccb959e 33 DigitalIn d22stat(p14); //Sensor left of the station
carlosperales95 7:e2b8461d4f05 34 //M2 - p15
carlosperales95 29:559eb2164488 35 DigitalIn station(p15); //Sensor in the middle of the station
carlosperales95 12:e914ca5cd44b 36
carlosperales95 7:e2b8461d4f05 37 //p16
carlosperales95 50:ee4398ee44be 38 //ENABLE - p17
carlosperales95 50:ee4398ee44be 39 DigitalOut enable(p17);
carlosperales95 12:e914ca5cd44b 40
carlosperales95 11:021210c59a95 41 //BUZZER - p18
carlosperales95 11:021210c59a95 42 DigitalOut buzz(p18); // buzz=0 doesn't beep, buzz=1 beeps
carlosperales95 7:e2b8461d4f05 43
carlosperales95 7:e2b8461d4f05 44 //POTENTIOMETER - p19
carlosperales95 13:dbf1ead12cee 45 AnalogIn pot(p19); //Gives float value pot.read(). Convert analog input to V with f*3.3
carlosperales95 7:e2b8461d4f05 46
carlosperales95 7:e2b8461d4f05 47 //DAT - p20
mglmx 3:fe7010b693a0 48 DigitalOut Track(p20); //Digital output bit used to drive track power via H-bridge
carlosperales95 7:e2b8461d4f05 49
carlosperales95 7:e2b8461d4f05 50 //LCD SCREEN - p21, p22, p23, p24, p25, p26
carlosperales95 11:021210c59a95 51 TextLCD lcd(p22,p21,p23,p24,p25,p26); // RS, E, A4, A5, A6, A7 // ldc.cls() to clear and printf(String up to 16char)
carlosperales95 7:e2b8461d4f05 52
carlosperales95 7:e2b8461d4f05 53 ///p27
carlosperales95 7:e2b8461d4f05 54 ///p28
mglmx 33:24ce12dec157 55 I2C i2c(p28,p27);
carlosperales95 7:e2b8461d4f05 56
carlosperales95 7:e2b8461d4f05 57 //LED1 - p29
mglmx 3:fe7010b693a0 58 DigitalOut redled(p29);
carlosperales95 7:e2b8461d4f05 59 //LED2 - p30
mglmx 3:fe7010b693a0 60 DigitalOut greenled(p30);
carlosperales95 7:e2b8461d4f05 61
carlosperales95 24:1d71dd8778c4 62 //MBED LEDS
mglmx 18:aa43bb62e60f 63 DigitalOut led1(LED1);
mglmx 18:aa43bb62e60f 64 DigitalOut led2(LED2);
mglmx 18:aa43bb62e60f 65 DigitalOut led3(LED3);
mglmx 16:2a2da0e67793 66
mglmx 16:2a2da0e67793 67 //MCP
mglmx 16:2a2da0e67793 68 MCP23017 *mcp;
carlosperales95 12:e914ca5cd44b 69
carlosperales95 24:1d71dd8778c4 70
carlosperales95 25:a42a1ed4d8e9 71 //------GLOBAL VARS
carlosperales95 25:a42a1ed4d8e9 72
carlosperales95 38:b9aba3715682 73 //......SENSOR POSITION VARS
carlosperales95 38:b9aba3715682 74
carlosperales95 45:d589318238bf 75 //Definition of D sensors, will be interpreted as ints for the program's logic
mglmx 34:c9ab2a987734 76 #define D0 0
mglmx 34:c9ab2a987734 77 #define D1 1
mglmx 34:c9ab2a987734 78 #define D2 2
mglmx 34:c9ab2a987734 79 #define D3 3
mglmx 34:c9ab2a987734 80 #define D4 4
mglmx 34:c9ab2a987734 81 #define D5 5
mglmx 34:c9ab2a987734 82 #define D6 6
mglmx 34:c9ab2a987734 83 #define D7 7
mglmx 34:c9ab2a987734 84 #define D8 8
mglmx 34:c9ab2a987734 85 #define D9 9
mglmx 34:c9ab2a987734 86 #define D10 10
mglmx 34:c9ab2a987734 87 #define D11 11
mglmx 34:c9ab2a987734 88 #define D12 12
mglmx 34:c9ab2a987734 89 #define D13 13
mglmx 34:c9ab2a987734 90 #define D21 14
mglmx 34:c9ab2a987734 91 #define D22 15
mglmx 34:c9ab2a987734 92
mglmx 41:4fa6aa29d1ed 93
mglmx 55:aa3baa01f43d 94
mglmx 55:aa3baa01f43d 95
carlosperales95 38:b9aba3715682 96 /**
carlosperales95 38:b9aba3715682 97 *
carlosperales95 38:b9aba3715682 98 *Position class.
carlosperales95 38:b9aba3715682 99 *
carlosperales95 38:b9aba3715682 100 *@position -
carlosperales95 38:b9aba3715682 101 *@previous_cw -
carlosperales95 38:b9aba3715682 102 *@previous_ccw -
carlosperales95 38:b9aba3715682 103 *
carlosperales95 38:b9aba3715682 104 *Position(int) -
carlosperales95 38:b9aba3715682 105 *
carlosperales95 38:b9aba3715682 106 *get_pos() -
carlosperales95 38:b9aba3715682 107 *get_prev_cw() -
carlosperales95 38:b9aba3715682 108 *get_ccw() -
carlosperales95 38:b9aba3715682 109 *add_prev_cw() -
carlosperales95 38:b9aba3715682 110 *add_ccw() -
carlosperales95 38:b9aba3715682 111 *
carlosperales95 38:b9aba3715682 112 **/
mglmx 35:cfcfeccb959e 113 class Position{
mglmx 35:cfcfeccb959e 114 private:
mglmx 35:cfcfeccb959e 115 int position;
mglmx 35:cfcfeccb959e 116 vector <int> previous_cw;
mglmx 35:cfcfeccb959e 117 vector <int> previous_ccw;
mglmx 35:cfcfeccb959e 118 public:
mglmx 35:cfcfeccb959e 119 Position(int p){
mglmx 35:cfcfeccb959e 120 position = p;
mglmx 35:cfcfeccb959e 121 }
mglmx 36:9428c72bdd58 122
mglmx 36:9428c72bdd58 123 int get_pos(){
mglmx 36:9428c72bdd58 124 return position;
mglmx 36:9428c72bdd58 125 }
mglmx 41:4fa6aa29d1ed 126
mglmx 41:4fa6aa29d1ed 127 vector<int> get_next_cw(){
mglmx 41:4fa6aa29d1ed 128 return previous_ccw;
mglmx 41:4fa6aa29d1ed 129 }
mglmx 41:4fa6aa29d1ed 130
mglmx 41:4fa6aa29d1ed 131 vector<int> get_next_ccw(){
mglmx 41:4fa6aa29d1ed 132 return previous_cw;
mglmx 41:4fa6aa29d1ed 133 }
carlosperales95 25:a42a1ed4d8e9 134
mglmx 35:cfcfeccb959e 135 vector <int> get_prev_cw(){
mglmx 35:cfcfeccb959e 136 return previous_cw;
mglmx 35:cfcfeccb959e 137 }
mglmx 35:cfcfeccb959e 138
mglmx 35:cfcfeccb959e 139 vector <int> get_prev_ccw(){
mglmx 35:cfcfeccb959e 140 return previous_ccw;
mglmx 35:cfcfeccb959e 141 }
mglmx 35:cfcfeccb959e 142
mglmx 35:cfcfeccb959e 143 void add_prev_cw(int pos){
mglmx 35:cfcfeccb959e 144 previous_cw.push_back(pos);
mglmx 35:cfcfeccb959e 145 };
mglmx 35:cfcfeccb959e 146
mglmx 35:cfcfeccb959e 147 void add_prev_ccw(int pos){
mglmx 35:cfcfeccb959e 148 previous_ccw.push_back(pos);
mglmx 35:cfcfeccb959e 149 };
mglmx 35:cfcfeccb959e 150 };
mglmx 35:cfcfeccb959e 151
carlosperales95 45:d589318238bf 152
mglmx 41:4fa6aa29d1ed 153 //Creating a vector with all the positions.
mglmx 41:4fa6aa29d1ed 154 vector<Position> positions;
mglmx 41:4fa6aa29d1ed 155
carlosperales95 45:d589318238bf 156
carlosperales95 45:d589318238bf 157 /**
carlosperales95 45:d589318238bf 158 *
carlosperales95 45:d589318238bf 159 *Train class.
carlosperales95 45:d589318238bf 160 *
carlosperales95 45:d589318238bf 161 *@position -
carlosperales95 45:d589318238bf 162 *@going_cw -
carlosperales95 45:d589318238bf 163 *
carlosperales95 45:d589318238bf 164 *Train(int, bool) -
carlosperales95 45:d589318238bf 165 *Train(bool) -
carlosperales95 45:d589318238bf 166 *
carlosperales95 45:d589318238bf 167 *Vector get_next_sensors() -
carlosperales95 45:d589318238bf 168 *set_position(int) -
carlosperales95 45:d589318238bf 169 *set_goes_cw(bool) -
carlosperales95 45:d589318238bf 170 *Position get_position() -
carlosperales95 45:d589318238bf 171 *Int get_position_number() -
carlosperales95 45:d589318238bf 172 *Bool goes_cw() -
carlosperales95 45:d589318238bf 173 *
carlosperales95 45:d589318238bf 174 **/
mglmx 41:4fa6aa29d1ed 175 class Train{
carlosperales95 45:d589318238bf 176
mglmx 41:4fa6aa29d1ed 177 private:
mglmx 55:aa3baa01f43d 178 int mode;
mglmx 41:4fa6aa29d1ed 179 Position *position;
mglmx 41:4fa6aa29d1ed 180 bool going_cw;
carlosperales95 45:d589318238bf 181
mglmx 41:4fa6aa29d1ed 182 public:
mglmx 41:4fa6aa29d1ed 183 Train(int pos, bool cw){
carlosperales95 45:d589318238bf 184
mglmx 41:4fa6aa29d1ed 185 position = &positions[pos];
mglmx 41:4fa6aa29d1ed 186 going_cw = cw;
mglmx 41:4fa6aa29d1ed 187 }
carlosperales95 45:d589318238bf 188
carlosperales95 45:d589318238bf 189 Train(bool cw){ going_cw = cw; }
mglmx 41:4fa6aa29d1ed 190
mglmx 41:4fa6aa29d1ed 191 vector<int> get_next_sensors(){
mglmx 41:4fa6aa29d1ed 192
mglmx 41:4fa6aa29d1ed 193 //Checking direction
mglmx 41:4fa6aa29d1ed 194 if(going_cw){
carlosperales95 45:d589318238bf 195
mglmx 43:346a1f4144cd 196 return position->get_next_cw();
mglmx 41:4fa6aa29d1ed 197 }else{
carlosperales95 45:d589318238bf 198
mglmx 43:346a1f4144cd 199 return position->get_next_ccw();
mglmx 41:4fa6aa29d1ed 200 }
mglmx 41:4fa6aa29d1ed 201 }
mglmx 41:4fa6aa29d1ed 202
mglmx 41:4fa6aa29d1ed 203 void set_position(int pos){
carlosperales95 45:d589318238bf 204
mglmx 41:4fa6aa29d1ed 205 position = &positions[pos]; //Taking the new position from the positions vector
mglmx 41:4fa6aa29d1ed 206 }
mglmx 41:4fa6aa29d1ed 207
mglmx 41:4fa6aa29d1ed 208 void set_goes_cw(bool cw){
carlosperales95 45:d589318238bf 209
mglmx 41:4fa6aa29d1ed 210 going_cw = cw;
mglmx 41:4fa6aa29d1ed 211 }
mglmx 41:4fa6aa29d1ed 212
mglmx 41:4fa6aa29d1ed 213 Position get_position(){
carlosperales95 45:d589318238bf 214
mglmx 41:4fa6aa29d1ed 215 return *position;
mglmx 41:4fa6aa29d1ed 216 }
mglmx 41:4fa6aa29d1ed 217
mglmx 41:4fa6aa29d1ed 218 int get_position_number(){
carlosperales95 45:d589318238bf 219
mglmx 41:4fa6aa29d1ed 220 return position->get_pos();
mglmx 41:4fa6aa29d1ed 221 }
mglmx 41:4fa6aa29d1ed 222
mglmx 41:4fa6aa29d1ed 223 bool goes_cw(){
carlosperales95 45:d589318238bf 224
mglmx 41:4fa6aa29d1ed 225 return going_cw;
mglmx 41:4fa6aa29d1ed 226 }
mglmx 41:4fa6aa29d1ed 227 };
mglmx 41:4fa6aa29d1ed 228
carlosperales95 45:d589318238bf 229
carlosperales95 38:b9aba3715682 230 //Creation of all the positions. One for every sensor on the table - Position name(mapping)
mglmx 35:cfcfeccb959e 231 Position d0(D0);
mglmx 35:cfcfeccb959e 232 Position d1(D1);
mglmx 35:cfcfeccb959e 233 Position d2(D2);
mglmx 35:cfcfeccb959e 234 Position d3(D3);
mglmx 35:cfcfeccb959e 235 Position d4(D4);
mglmx 35:cfcfeccb959e 236 Position d5(D5);
mglmx 35:cfcfeccb959e 237 Position d6(D6);
mglmx 35:cfcfeccb959e 238 Position d7(D7);
mglmx 35:cfcfeccb959e 239 Position d8(D8);
mglmx 35:cfcfeccb959e 240 Position d9(D9);
mglmx 35:cfcfeccb959e 241 Position d10(D10);
mglmx 35:cfcfeccb959e 242 Position d11(D11);
mglmx 35:cfcfeccb959e 243 Position d12(D12);
mglmx 35:cfcfeccb959e 244 Position d13(D13);
mglmx 35:cfcfeccb959e 245 Position d21(D21);
mglmx 35:cfcfeccb959e 246 Position d22(D22);
mglmx 35:cfcfeccb959e 247
carlosperales95 45:d589318238bf 248 /**
carlosperales95 45:d589318238bf 249 *Defining areas for train detection and collision logic.
carlosperales95 45:d589318238bf 250 *area_A_arr/area_B_arr - Arrays that hold the Dsensors for each area, used to initialize the vectors.
carlosperales95 45:d589318238bf 251 *area_A/area_B - Vectors that hold the different sensors of the corresponding areas of the track.
carlosperales95 45:d589318238bf 252 **/
mglmx 43:346a1f4144cd 253 int area_A_arr[] = {D21,D2,D22,D1,D0,D13,D12};
mglmx 43:346a1f4144cd 254 int area_B_arr[] = {D6,D7,D8};
mglmx 43:346a1f4144cd 255
mglmx 43:346a1f4144cd 256 const vector<int> area_A(area_A_arr,area_A_arr + sizeof(area_A_arr) / sizeof(int));
mglmx 43:346a1f4144cd 257 const vector<int> area_B(area_B_arr,area_B_arr + sizeof(area_B_arr) / sizeof(int));
mglmx 43:346a1f4144cd 258
mglmx 41:4fa6aa29d1ed 259
mglmx 35:cfcfeccb959e 260
carlosperales95 25:a42a1ed4d8e9 261 //.....DCC TRAIN COMMAND VARS
carlosperales95 25:a42a1ed4d8e9 262
carlosperales95 25:a42a1ed4d8e9 263 //typical out of box default engine DCC address is 3 (at least for Bachmann trains)
carlosperales95 25:a42a1ed4d8e9 264 //Note: A DCC controller can reprogram the address whenever needed
mglmx 32:e5b732fb8e65 265 const unsigned int DCCaddressDR = 0x01; //Address for train 1 DARK-RED
mglmx 32:e5b732fb8e65 266 const unsigned int DCCaddressLR = 0x03; //Address for train 3 LIGHT-RED
carlosperales95 25:a42a1ed4d8e9 267
carlosperales95 25:a42a1ed4d8e9 268 //01DCSSSS for speed, D is direction (fwd=1 and rev=0), C is speed(SSSSC) LSB
carlosperales95 25:a42a1ed4d8e9 269 const unsigned int DCCinst_forward = 0x68; //forward half speed
mglmx 51:badef9fc202f 270 const unsigned int DCCinst_forward_slow = 0x66; //forward slow speed (step 9)
mglmx 51:badef9fc202f 271 const unsigned int DCCinst_forward_fast = 0x6C; //Forward fast speed (step 22)
mglmx 51:badef9fc202f 272 const unsigned int DCCinst_forward_full = 0x6F; //Forward full speed
carlosperales95 25:a42a1ed4d8e9 273 const unsigned int DCCinst_reverse = 0x48; //reverse half speed
carlosperales95 25:a42a1ed4d8e9 274 const unsigned int DCCinst_stop = 0x50; //stop the train
carlosperales95 25:a42a1ed4d8e9 275
carlosperales95 25:a42a1ed4d8e9 276 //100DDDDD for basic headlight functions
carlosperales95 25:a42a1ed4d8e9 277 const unsigned int DCC_func_lighton = 0x90; //F0 turns on headlight function
carlosperales95 25:a42a1ed4d8e9 278 const unsigned int DCC_func_dimlight = 0x91; //F0 + F1 dims headlight
carlosperales95 25:a42a1ed4d8e9 279
carlosperales95 25:a42a1ed4d8e9 280
carlosperales95 25:a42a1ed4d8e9 281 //.....SWITCH COMMAND VARS
carlosperales95 25:a42a1ed4d8e9 282
carlosperales95 25:a42a1ed4d8e9 283 const unsigned int SWBaddress = 0x06; //Address for switch box
carlosperales95 25:a42a1ed4d8e9 284
carlosperales95 25:a42a1ed4d8e9 285 //100DDDDD where DDDDD is the switch command and 100 is constant:
carlosperales95 25:a42a1ed4d8e9 286
carlosperales95 25:a42a1ed4d8e9 287 //00001(F1 active)-00010(F2 active)-00100(F3 active)-01000(F4 active)
carlosperales95 25:a42a1ed4d8e9 288 //Example - 111111 0 00000101 0 10000000 0 10000101 1 - idle
carlosperales95 25:a42a1ed4d8e9 289 const unsigned int SWBidle = 0x80; //IDLE - Flip last activated SW.
carlosperales95 25:a42a1ed4d8e9 290 const unsigned int SWBflip_1 = 0x81; //Flip SW1
carlosperales95 25:a42a1ed4d8e9 291 const unsigned int SWBflip_2 = 0x82; //Flip SW2
carlosperales95 25:a42a1ed4d8e9 292 const unsigned int SWBflip_3 = 0x84; //Flip SW3
carlosperales95 25:a42a1ed4d8e9 293 const unsigned int SWBflip_4 = 0x88; //Flip SW4
carlosperales95 25:a42a1ed4d8e9 294
carlosperales95 40:9acc1341456a 295
carlosperales95 45:d589318238bf 296 /**
carlosperales95 45:d589318238bf 297 *Creation of 2 Train objects.
carlosperales95 45:d589318238bf 298 *Using boolean constructor because position initialization will be done after initializing all position vectors.
carlosperales95 45:d589318238bf 299 *DR_train = Dark Red train - LR_train = Light Red Train
carlosperales95 45:d589318238bf 300 **/
mglmx 55:aa3baa01f43d 301 Train DR_train(true); //Position and going_cw
mglmx 43:346a1f4144cd 302 Train LR_train(true);
mglmx 43:346a1f4144cd 303
carlosperales95 47:e992a129ef44 304 //possibility of an array having {dr_train, lr_train}? for reuse and modularity of functions
carlosperales95 47:e992a129ef44 305
carlosperales95 47:e992a129ef44 306
carlosperales95 45:d589318238bf 307 /**
carlosperales95 45:d589318238bf 308 *Booleans that will determine if the train should be moving or not.
carlosperales95 45:d589318238bf 309 *Booleans will switch to false to stop any of the trains and avoid collisions.
carlosperales95 45:d589318238bf 310 *DR_run - Boolean for DR_train / LR_run - Boolean for LR_train
carlosperales95 45:d589318238bf 311 **/
mglmx 43:346a1f4144cd 312 bool DR_run = true;
mglmx 43:346a1f4144cd 313 bool LR_run = true;
mglmx 18:aa43bb62e60f 314
carlosperales95 38:b9aba3715682 315
carlosperales95 45:d589318238bf 316
carlosperales95 38:b9aba3715682 317 //**************** FUNCTIONS FOR DENVER TRAIN ****************//
carlosperales95 38:b9aba3715682 318
carlosperales95 38:b9aba3715682 319
mglmx 22:e4153ca757dd 320 /**
carlosperales95 45:d589318238bf 321 *
carlosperales95 25:a42a1ed4d8e9 322 *Activates the buzzer for 0.5 seconds.
carlosperales95 45:d589318238bf 323 *
mglmx 22:e4153ca757dd 324 **/
mglmx 22:e4153ca757dd 325 void doBuzz(){
carlosperales95 25:a42a1ed4d8e9 326
mglmx 22:e4153ca757dd 327 buzz = 1;
mglmx 22:e4153ca757dd 328 wait(0.5);
mglmx 22:e4153ca757dd 329 buzz = 0;
mglmx 22:e4153ca757dd 330 }
mglmx 18:aa43bb62e60f 331
carlosperales95 45:d589318238bf 332
carlosperales95 45:d589318238bf 333 /**
carlosperales95 45:d589318238bf 334 *
carlosperales95 47:e992a129ef44 335 *Initializes every position's vectors (prev_cw and prev_ccw) with the corresponding sensors.
carlosperales95 47:e992a129ef44 336 *prev_cw - Sensors previous to the current in clockwise sense.
carlosperales95 47:e992a129ef44 337 *prev_ccw - Sensors previous to the current in counter-clockwise sense.
carlosperales95 45:d589318238bf 338 *
carlosperales95 45:d589318238bf 339 **/
mglmx 35:cfcfeccb959e 340 void init_positions(){
mglmx 36:9428c72bdd58 341
mglmx 35:cfcfeccb959e 342 d0.add_prev_cw(D1);
mglmx 35:cfcfeccb959e 343 d0.add_prev_ccw(D13);
mglmx 35:cfcfeccb959e 344
mglmx 35:cfcfeccb959e 345 d1.add_prev_cw(D22);
mglmx 35:cfcfeccb959e 346 d1.add_prev_ccw(D0);
mglmx 35:cfcfeccb959e 347
mglmx 35:cfcfeccb959e 348 d22.add_prev_cw(D2);
mglmx 35:cfcfeccb959e 349 d22.add_prev_ccw(D1);
mglmx 35:cfcfeccb959e 350
mglmx 35:cfcfeccb959e 351 d2.add_prev_cw(D21);
mglmx 35:cfcfeccb959e 352 d2.add_prev_ccw(D22);
mglmx 35:cfcfeccb959e 353
mglmx 35:cfcfeccb959e 354 d21.add_prev_cw(D3);
mglmx 35:cfcfeccb959e 355 d21.add_prev_cw(D4);
mglmx 35:cfcfeccb959e 356 d21.add_prev_ccw(D2);
mglmx 35:cfcfeccb959e 357
mglmx 35:cfcfeccb959e 358 d3.add_prev_cw(D9);
mglmx 35:cfcfeccb959e 359 d3.add_prev_ccw(D21);
mglmx 35:cfcfeccb959e 360
mglmx 35:cfcfeccb959e 361 d4.add_prev_cw(D6);
mglmx 35:cfcfeccb959e 362 d4.add_prev_ccw(D21);
mglmx 35:cfcfeccb959e 363
mglmx 35:cfcfeccb959e 364 d5.add_prev_cw(D6);
mglmx 35:cfcfeccb959e 365 d5.add_prev_ccw(D11);
mglmx 35:cfcfeccb959e 366
mglmx 35:cfcfeccb959e 367 d6.add_prev_cw(D7);
mglmx 35:cfcfeccb959e 368 d6.add_prev_ccw(D4);
mglmx 35:cfcfeccb959e 369 d6.add_prev_ccw(D5);
mglmx 35:cfcfeccb959e 370
mglmx 35:cfcfeccb959e 371 d7.add_prev_cw(D8);
mglmx 35:cfcfeccb959e 372 d7.add_prev_ccw(D6);
mglmx 35:cfcfeccb959e 373
mglmx 36:9428c72bdd58 374 d8.add_prev_cw(D9);
mglmx 36:9428c72bdd58 375 d8.add_prev_cw(D10);
mglmx 36:9428c72bdd58 376 d8.add_prev_ccw(D7);
mglmx 36:9428c72bdd58 377
mglmx 35:cfcfeccb959e 378 d9.add_prev_cw(D3);
mglmx 35:cfcfeccb959e 379 d9.add_prev_ccw(D8);
mglmx 35:cfcfeccb959e 380
mglmx 35:cfcfeccb959e 381 d10.add_prev_cw(D12);
mglmx 35:cfcfeccb959e 382 d10.add_prev_ccw(D8);
mglmx 35:cfcfeccb959e 383
mglmx 35:cfcfeccb959e 384 d11.add_prev_cw(D12);
mglmx 35:cfcfeccb959e 385 d11.add_prev_ccw(D5);
mglmx 35:cfcfeccb959e 386
mglmx 35:cfcfeccb959e 387 d12.add_prev_cw(D13);
mglmx 35:cfcfeccb959e 388 d12.add_prev_ccw(D10);
mglmx 35:cfcfeccb959e 389 d12.add_prev_ccw(D11);
mglmx 35:cfcfeccb959e 390
mglmx 35:cfcfeccb959e 391 d13.add_prev_cw(D0);
mglmx 35:cfcfeccb959e 392 d13.add_prev_ccw(D12);
mglmx 36:9428c72bdd58 393
mglmx 36:9428c72bdd58 394 //Initialize array with positions
mglmx 36:9428c72bdd58 395 positions.push_back(d0);
mglmx 36:9428c72bdd58 396 positions.push_back(d1);
mglmx 36:9428c72bdd58 397 positions.push_back(d2);
mglmx 36:9428c72bdd58 398 positions.push_back(d3);
mglmx 36:9428c72bdd58 399 positions.push_back(d4);
mglmx 36:9428c72bdd58 400 positions.push_back(d5);
mglmx 36:9428c72bdd58 401 positions.push_back(d6);
mglmx 36:9428c72bdd58 402 positions.push_back(d7);
mglmx 36:9428c72bdd58 403 positions.push_back(d8);
mglmx 36:9428c72bdd58 404 positions.push_back(d9);
mglmx 36:9428c72bdd58 405 positions.push_back(d10);
mglmx 36:9428c72bdd58 406 positions.push_back(d11);
mglmx 36:9428c72bdd58 407 positions.push_back(d12);
mglmx 36:9428c72bdd58 408 positions.push_back(d13);
mglmx 36:9428c72bdd58 409 positions.push_back(d21);
mglmx 36:9428c72bdd58 410 positions.push_back(d22);
mglmx 35:cfcfeccb959e 411 }
carlosperales95 24:1d71dd8778c4 412
carlosperales95 45:d589318238bf 413
mglmx 22:e4153ca757dd 414 /**
carlosperales95 24:1d71dd8778c4 415 *
carlosperales95 28:71bd4c83c05f 416 *Here we initialize the mcp that will be used to manage the interrupts.
carlosperales95 25:a42a1ed4d8e9 417 *
carlosperales95 24:1d71dd8778c4 418 **/
mglmx 18:aa43bb62e60f 419 void initialize_mcp(){
mglmx 33:24ce12dec157 420 mcp = new MCP23017(i2c,0x40); //Connect to SCL - p28 and SDA - p27 and MPC I2C address 0x40
mglmx 18:aa43bb62e60f 421
mglmx 33:24ce12dec157 422 mcp->_write(IODIRA, (unsigned char )0xff);
mglmx 33:24ce12dec157 423 mcp->_write(IODIRB, (unsigned char )0xff);
mglmx 33:24ce12dec157 424 mcp->_write(IPOLA, (unsigned char )0x00);
mglmx 33:24ce12dec157 425 mcp->_write(IPOLB, (unsigned char )0x00);
mglmx 33:24ce12dec157 426 mcp->_write(DEFVALA, (unsigned char )0xff);
mglmx 33:24ce12dec157 427 mcp->_write(DEFVALB, (unsigned char )0xff);
mglmx 33:24ce12dec157 428 mcp->_write(INTCONA, (unsigned char )0xff);
mglmx 33:24ce12dec157 429 mcp->_write(INTCONB, (unsigned char )0xff);
mglmx 33:24ce12dec157 430 mcp->_write(IOCONA, (unsigned char )0x2);
mglmx 33:24ce12dec157 431 mcp->_write(IOCONB, (unsigned char )0x2);
mglmx 33:24ce12dec157 432 mcp->_write(GPPUA, (unsigned char )0xff);
mglmx 33:24ce12dec157 433 mcp->_write(GPPUB, (unsigned char )0xff);
mglmx 33:24ce12dec157 434
mglmx 18:aa43bb62e60f 435 }
mglmx 18:aa43bb62e60f 436
carlosperales95 45:d589318238bf 437
carlosperales95 24:1d71dd8778c4 438 /**
carlosperales95 24:1d71dd8778c4 439 *
carlosperales95 37:bb15bea420a3 440 *Returns the number of the sensor where the train was detected.
carlosperales95 37:bb15bea420a3 441 *
carlosperales95 47:e992a129ef44 442 *@number -
carlosperales95 47:e992a129ef44 443 *@interrupt -
carlosperales95 47:e992a129ef44 444 *
carlosperales95 37:bb15bea420a3 445 **/
mglmx 34:c9ab2a987734 446 int get_sensor(unsigned int number,int interrupt){
carlosperales95 45:d589318238bf 447
mglmx 34:c9ab2a987734 448 int sensor = -1;
mglmx 34:c9ab2a987734 449
carlosperales95 37:bb15bea420a3 450 for(int i=0; i<8; i++){
mglmx 34:c9ab2a987734 451
carlosperales95 45:d589318238bf 452 if(~number & 1<<i){
carlosperales95 45:d589318238bf 453
mglmx 34:c9ab2a987734 454 sensor = i;
mglmx 34:c9ab2a987734 455 }
mglmx 34:c9ab2a987734 456 }
mglmx 34:c9ab2a987734 457
mglmx 34:c9ab2a987734 458 if(interrupt == 1){
carlosperales95 45:d589318238bf 459
carlosperales95 37:bb15bea420a3 460 sensor+= 8; // Sensors caught by interreupt1 are identified from 8 to 15.
mglmx 34:c9ab2a987734 461 }
carlosperales95 45:d589318238bf 462
mglmx 34:c9ab2a987734 463 return sensor;
mglmx 34:c9ab2a987734 464 }
mglmx 34:c9ab2a987734 465
carlosperales95 45:d589318238bf 466
carlosperales95 45:d589318238bf 467 /**
carlosperales95 45:d589318238bf 468 *
carlosperales95 47:e992a129ef44 469 *Checks if the element exists within the vector.
carlosperales95 45:d589318238bf 470 *
carlosperales95 47:e992a129ef44 471 *@v - The vector (of ints) the method will go through.
carlosperales95 47:e992a129ef44 472 *@element - The element the method will look for.
carlosperales95 45:d589318238bf 473 *
carlosperales95 45:d589318238bf 474 **/
mglmx 43:346a1f4144cd 475 bool in_vector(vector<int>v,int element){
carlosperales95 45:d589318238bf 476
mglmx 43:346a1f4144cd 477 bool exist = false;
mglmx 43:346a1f4144cd 478
mglmx 43:346a1f4144cd 479 for(int i=0; i< v.size(); i++){
carlosperales95 45:d589318238bf 480
mglmx 43:346a1f4144cd 481 if(v[i] == element){
carlosperales95 45:d589318238bf 482
mglmx 43:346a1f4144cd 483 exist = true;
mglmx 43:346a1f4144cd 484 }
carlosperales95 47:e992a129ef44 485 }
mglmx 43:346a1f4144cd 486 return exist;
mglmx 43:346a1f4144cd 487 }
mglmx 43:346a1f4144cd 488
carlosperales95 45:d589318238bf 489
carlosperales95 42:b445252a772a 490 /**
carlosperales95 47:e992a129ef44 491 *This method will checks if there is a non-avoidable frontal collision(NAFC).
carlosperales95 47:e992a129ef44 492 *A NAFC will happen if:
carlosperales95 47:e992a129ef44 493 *
carlosperales95 47:e992a129ef44 494 *Both trains in area A or B with different direction
carlosperales95 47:e992a129ef44 495 *Trains in (D11 and D5) or (D9 and D3) with same direction
carlosperales95 47:e992a129ef44 496 *
mglmx 43:346a1f4144cd 497 */
mglmx 43:346a1f4144cd 498 bool check_NAC(bool DR_in_A, bool DR_in_B,bool LR_in_A,bool LR_in_B){
carlosperales95 45:d589318238bf 499
mglmx 43:346a1f4144cd 500 bool NAC = false;
mglmx 43:346a1f4144cd 501
mglmx 43:346a1f4144cd 502 if((DR_in_A && LR_in_A) || (DR_in_B && LR_in_B) ){ //Check if both are in same area
carlosperales95 45:d589318238bf 503
mglmx 43:346a1f4144cd 504 if(DR_train.goes_cw() ^ LR_train.goes_cw()){ //XOR: They must have different values to be true (Different direction)
carlosperales95 45:d589318238bf 505
mglmx 43:346a1f4144cd 506 NAC = true;
mglmx 43:346a1f4144cd 507 }
mglmx 43:346a1f4144cd 508 }else if((DR_train.get_position_number() == D11) && (LR_train.get_position_number() == D5 )){ //Check if they are in position D11 and D5
carlosperales95 45:d589318238bf 509
mglmx 43:346a1f4144cd 510 if(!(DR_train.goes_cw() ^ LR_train.goes_cw())){ // NOT XOR: They must have same values to be true (Same direction)
carlosperales95 45:d589318238bf 511
mglmx 43:346a1f4144cd 512 NAC = true;
mglmx 43:346a1f4144cd 513 }
mglmx 43:346a1f4144cd 514 }else if((DR_train.get_position_number() == D9) && (LR_train.get_position_number() == D3 )){//Check if they are in position D9 and D3
carlosperales95 45:d589318238bf 515
mglmx 43:346a1f4144cd 516 if(!(DR_train.goes_cw() ^ LR_train.goes_cw())){ // NOT XOR: They must have same values to be true (Same direction)
carlosperales95 45:d589318238bf 517
mglmx 43:346a1f4144cd 518 NAC = true;
mglmx 43:346a1f4144cd 519 }
mglmx 43:346a1f4144cd 520 }
carlosperales95 45:d589318238bf 521 return NAC;
mglmx 43:346a1f4144cd 522 }
mglmx 43:346a1f4144cd 523
mglmx 55:aa3baa01f43d 524 /*
mglmx 55:aa3baa01f43d 525 void AFC_action(int switch_n, int sensor, Train *stop_train, Train * cont_train ){
mglmx 55:aa3baa01f43d 526
mglmx 55:aa3baa01f43d 527 flip_switch(switch_n,5); //Activate switch 5
mglmx 55:aa3baa01f43d 528
mglmx 55:aa3baa01f43d 529 while(cont_train->get_position_number() != sensor){
mglmx 55:aa3baa01f43d 530 DR_run = false; //DR has to stop
mglmx 55:aa3baa01f43d 531 LR_run = true; //LR continues
mglmx 55:aa3baa01f43d 532 send_command();
mglmx 55:aa3baa01f43d 533 }
mglmx 55:aa3baa01f43d 534 DR_run = true;
mglmx 55:aa3baa01f43d 535 LR_run = true; //Both trains start to run again
mglmx 55:aa3baa01f43d 536 }
mglmx 55:aa3baa01f43d 537 */
carlosperales95 45:d589318238bf 538
mglmx 43:346a1f4144cd 539 /**
carlosperales95 47:e992a129ef44 540 *
mglmx 55:aa3baa01f43d 541 *The function will check if there is an Avoidable Frontal Collision (AFC).
carlosperales95 47:e992a129ef44 542 *AFC will occur if:
carlosperales95 47:e992a129ef44 543 *
carlosperales95 47:e992a129ef44 544 *Train in area A(ccw) and train in D4(cw)
carlosperales95 47:e992a129ef44 545 *Train in area A(cw) and train in D10(ccw)
carlosperales95 47:e992a129ef44 546 *Train in area B(cw) and train in D4(ccw)
carlosperales95 47:e992a129ef44 547 *Train in area B(ccw) and train in D10(ccw)
carlosperales95 47:e992a129ef44 548 *
carlosperales95 46:7a0933676b13 549 **/
carlosperales95 46:7a0933676b13 550 bool check_AFC(bool DR_in_A, bool DR_in_B,bool LR_in_A,bool LR_in_B){ //TODO - Add same for LR train
carlosperales95 45:d589318238bf 551
carlosperales95 45:d589318238bf 552 if( DR_train.get_position_number() == D4){
carlosperales95 45:d589318238bf 553
mglmx 43:346a1f4144cd 554 if(DR_train.goes_cw()){
carlosperales95 45:d589318238bf 555
mglmx 43:346a1f4144cd 556 if(LR_in_A && !LR_train.goes_cw()){
carlosperales95 45:d589318238bf 557
mglmx 43:346a1f4144cd 558 //Activate switch2
mglmx 43:346a1f4144cd 559 //DR_train has to stop
mglmx 43:346a1f4144cd 560 //When LR is at D3 DR continues
mglmx 43:346a1f4144cd 561 }
mglmx 43:346a1f4144cd 562 }else{ //DR goes ccw
carlosperales95 45:d589318238bf 563
mglmx 43:346a1f4144cd 564 if(LR_in_B && LR_train.goes_cw()){
carlosperales95 45:d589318238bf 565
mglmx 43:346a1f4144cd 566 //DR_train stops
mglmx 43:346a1f4144cd 567 //Activate switch3
mglmx 43:346a1f4144cd 568 //When LR is at D5 DR continues
mglmx 43:346a1f4144cd 569 }
mglmx 43:346a1f4144cd 570 }
mglmx 43:346a1f4144cd 571
mglmx 43:346a1f4144cd 572 }else if(DR_train.get_position_number() == D10){
carlosperales95 45:d589318238bf 573
mglmx 43:346a1f4144cd 574 if(DR_train.goes_cw()){
carlosperales95 45:d589318238bf 575
mglmx 43:346a1f4144cd 576 if(LR_in_B && !LR_train.goes_cw()){
carlosperales95 45:d589318238bf 577
mglmx 43:346a1f4144cd 578 //DR train stops
mglmx 43:346a1f4144cd 579 //Activate switch4
mglmx 43:346a1f4144cd 580 //When LR is at D9 DR continues
mglmx 43:346a1f4144cd 581 }
mglmx 43:346a1f4144cd 582 }else{
carlosperales95 45:d589318238bf 583
mglmx 43:346a1f4144cd 584 if(LR_in_A && LR_train.goes_cw()){
carlosperales95 45:d589318238bf 585
mglmx 43:346a1f4144cd 586 //DR train stops
mglmx 43:346a1f4144cd 587 //Activate switch1
mglmx 43:346a1f4144cd 588 //When LR is at D9 DR continues
mglmx 43:346a1f4144cd 589 }
mglmx 43:346a1f4144cd 590 }
mglmx 43:346a1f4144cd 591 }
mglmx 43:346a1f4144cd 592 }
mglmx 43:346a1f4144cd 593
carlosperales95 45:d589318238bf 594
carlosperales95 45:d589318238bf 595 /**
carlosperales95 45:d589318238bf 596 *
carlosperales95 47:e992a129ef44 597 *The method check_position will check if any of the trains is in any of the areas.
carlosperales95 47:e992a129ef44 598 *It will go through all the area vectors (A,B) and call the function in_vector to check inside the vectors.
carlosperales95 45:d589318238bf 599 *
carlosperales95 45:d589318238bf 600 **/
mglmx 43:346a1f4144cd 601 void check_position(){
carlosperales95 45:d589318238bf 602
mglmx 43:346a1f4144cd 603 bool DR_in_A, DR_in_B, LR_in_A, LR_in_B;
mglmx 43:346a1f4144cd 604
carlosperales95 47:e992a129ef44 605 DR_in_A = in_vector(area_A,DR_train.get_position_number()); //Check if DR train is in area A
carlosperales95 47:e992a129ef44 606 DR_in_B = in_vector(area_B,DR_train.get_position_number());
carlosperales95 47:e992a129ef44 607 LR_in_A = in_vector(area_A,LR_train.get_position_number());
mglmx 55:aa3baa01f43d 608 LR_in_B = in_vector(area_B,LR_train.get_position_number());
mglmx 55:aa3baa01f43d 609
mglmx 55:aa3baa01f43d 610 if(check_NAC(DR_in_A,DR_in_B,LR_in_A,LR_in_B)){
mglmx 55:aa3baa01f43d 611 lcd.cls();
mglmx 55:aa3baa01f43d 612 lcd.printf("NAC!!!");
mglmx 55:aa3baa01f43d 613 }
mglmx 43:346a1f4144cd 614
mglmx 43:346a1f4144cd 615 }
mglmx 43:346a1f4144cd 616
carlosperales95 45:d589318238bf 617
carlosperales95 45:d589318238bf 618 /**
carlosperales95 45:d589318238bf 619 *
carlosperales95 47:e992a129ef44 620 *Description
carlosperales95 45:d589318238bf 621 *
carlosperales95 47:e992a129ef44 622 *@sensor -
carlosperales95 45:d589318238bf 623 *
carlosperales95 45:d589318238bf 624 **/
mglmx 35:cfcfeccb959e 625 void update_train_pos(int sensor){
mglmx 52:c08495446f87 626 led2 = 1;
mglmx 41:4fa6aa29d1ed 627
mglmx 35:cfcfeccb959e 628 bool found_DR = false;
mglmx 41:4fa6aa29d1ed 629 bool found_LR = false;
mglmx 41:4fa6aa29d1ed 630
mglmx 49:880c0b9c9c64 631 string DR_dir,LR_dir;
mglmx 49:880c0b9c9c64 632
mglmx 49:880c0b9c9c64 633 if(DR_train.goes_cw()){
mglmx 49:880c0b9c9c64 634 DR_dir = "cw";
mglmx 49:880c0b9c9c64 635 }else{
mglmx 49:880c0b9c9c64 636 DR_dir = "ccw";
mglmx 49:880c0b9c9c64 637 }
mglmx 49:880c0b9c9c64 638
mglmx 49:880c0b9c9c64 639 if(LR_train.goes_cw()){
mglmx 49:880c0b9c9c64 640 LR_dir = "cw";
mglmx 49:880c0b9c9c64 641 }else{
mglmx 49:880c0b9c9c64 642 LR_dir = "ccw";
mglmx 55:aa3baa01f43d 643 }
mglmx 49:880c0b9c9c64 644
mglmx 49:880c0b9c9c64 645
mglmx 52:c08495446f87 646 //wait(0.7);
mglmx 41:4fa6aa29d1ed 647
mglmx 55:aa3baa01f43d 648 if(sensor == DR_train.get_position_number() || sensor == LR_train.get_position_number()){
mglmx 55:aa3baa01f43d 649 led2 = 0;
mglmx 52:c08495446f87 650 }else{
mglmx 55:aa3baa01f43d 651
mglmx 55:aa3baa01f43d 652 lcd.cls();
mglmx 55:aa3baa01f43d 653 lcd.printf("S:D%d DR%d(",sensor,DR_train.get_position_number());
mglmx 55:aa3baa01f43d 654
mglmx 55:aa3baa01f43d 655 //TODO: Do a for to print all next sensors.
mglmx 55:aa3baa01f43d 656 for(int i=0; i<DR_train.get_next_sensors().size(); i++){
mglmx 55:aa3baa01f43d 657
mglmx 55:aa3baa01f43d 658 lcd.printf("%d,",DR_train.get_next_sensors()[i]);
mglmx 55:aa3baa01f43d 659 }
mglmx 55:aa3baa01f43d 660
mglmx 55:aa3baa01f43d 661 lcd.printf(")%s LR%d(",DR_dir,LR_train.get_position_number());
mglmx 55:aa3baa01f43d 662
mglmx 55:aa3baa01f43d 663 for(int i=0; i<LR_train.get_next_sensors().size(); i++){
mglmx 55:aa3baa01f43d 664
mglmx 55:aa3baa01f43d 665 lcd.printf("%d,",LR_train.get_next_sensors()[i]);
mglmx 55:aa3baa01f43d 666 }
mglmx 55:aa3baa01f43d 667
mglmx 55:aa3baa01f43d 668 lcd.printf(")%s",LR_dir);
mglmx 55:aa3baa01f43d 669
mglmx 52:c08495446f87 670 //Checking next sensors for DR train
mglmx 55:aa3baa01f43d 671 for(int i=0; i<DR_train.get_next_sensors().size(); i++){
mglmx 55:aa3baa01f43d 672
mglmx 55:aa3baa01f43d 673 if(DR_train.get_next_sensors()[i] == sensor){ //If the sensor is one expected to visit by the train we update the position
carlosperales95 37:bb15bea420a3 674
mglmx 55:aa3baa01f43d 675 found_DR = true;
mglmx 55:aa3baa01f43d 676
mglmx 55:aa3baa01f43d 677
mglmx 55:aa3baa01f43d 678 if(DR_train.goes_cw()){
mglmx 55:aa3baa01f43d 679 if(DR_train.get_position_number() == D5 || DR_train.get_position_number() == D11){
mglmx 55:aa3baa01f43d 680
mglmx 55:aa3baa01f43d 681 DR_train.set_goes_cw(false); //If train goes cw and passes D5 or D11 we change orientation
mglmx 55:aa3baa01f43d 682 }
mglmx 55:aa3baa01f43d 683 }else{
mglmx 55:aa3baa01f43d 684
mglmx 55:aa3baa01f43d 685 if(DR_train.get_position_number() == D9 || DR_train.get_position_number() == D3){
mglmx 55:aa3baa01f43d 686
mglmx 55:aa3baa01f43d 687 DR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
mglmx 55:aa3baa01f43d 688 }
mglmx 55:aa3baa01f43d 689 }
mglmx 55:aa3baa01f43d 690
mglmx 55:aa3baa01f43d 691 DR_train.set_position(sensor);
mglmx 55:aa3baa01f43d 692
mglmx 55:aa3baa01f43d 693 }
mglmx 55:aa3baa01f43d 694 }
mglmx 55:aa3baa01f43d 695
mglmx 55:aa3baa01f43d 696 //Checking next sensors for LR train
mglmx 55:aa3baa01f43d 697 for(int i=0; i<LR_train.get_next_sensors().size(); i++){
mglmx 55:aa3baa01f43d 698
mglmx 55:aa3baa01f43d 699 if(LR_train.get_next_sensors()[i] == sensor){
carlosperales95 45:d589318238bf 700
mglmx 55:aa3baa01f43d 701 found_LR = true;
mglmx 55:aa3baa01f43d 702
mglmx 55:aa3baa01f43d 703 if(LR_train.goes_cw()){
carlosperales95 45:d589318238bf 704
mglmx 55:aa3baa01f43d 705 if(LR_train.get_position_number() == D5 || LR_train.get_position_number() == D11){
mglmx 55:aa3baa01f43d 706 LR_train.set_goes_cw(false); //If train goes cw and passes D5 or D11 we change orientation
mglmx 55:aa3baa01f43d 707 }
mglmx 55:aa3baa01f43d 708 }else{
mglmx 55:aa3baa01f43d 709
mglmx 55:aa3baa01f43d 710 if(LR_train.get_position_number() == D9 || LR_train.get_position_number() == D3 ){
mglmx 55:aa3baa01f43d 711
mglmx 55:aa3baa01f43d 712 LR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
mglmx 55:aa3baa01f43d 713 }
mglmx 41:4fa6aa29d1ed 714 }
mglmx 55:aa3baa01f43d 715 LR_train.set_position(sensor);
mglmx 55:aa3baa01f43d 716
mglmx 55:aa3baa01f43d 717 }
mglmx 55:aa3baa01f43d 718 }
mglmx 55:aa3baa01f43d 719 /*
mglmx 55:aa3baa01f43d 720 if(found_DR){
mglmx 49:880c0b9c9c64 721
mglmx 55:aa3baa01f43d 722 //doBuzz();
mglmx 55:aa3baa01f43d 723 lcd.cls();
mglmx 55:aa3baa01f43d 724 lcd.printf("DR is at D%d",DR_train.get_position_number());
mglmx 41:4fa6aa29d1ed 725 }
mglmx 55:aa3baa01f43d 726
mglmx 55:aa3baa01f43d 727 if(found_LR){
mglmx 55:aa3baa01f43d 728
mglmx 55:aa3baa01f43d 729 lcd.cls();
mglmx 55:aa3baa01f43d 730 lcd.printf("LR is at D%d",LR_train.get_position_number());
mglmx 55:aa3baa01f43d 731 }
mglmx 55:aa3baa01f43d 732
mglmx 55:aa3baa01f43d 733 if(!found_DR && !found_LR){
mglmx 55:aa3baa01f43d 734
mglmx 55:aa3baa01f43d 735 lcd.cls();
mglmx 55:aa3baa01f43d 736 lcd.printf("No train before :(");
mglmx 55:aa3baa01f43d 737 }
mglmx 55:aa3baa01f43d 738 */
mglmx 55:aa3baa01f43d 739
mglmx 41:4fa6aa29d1ed 740 }
mglmx 41:4fa6aa29d1ed 741
mglmx 35:cfcfeccb959e 742 }
mglmx 35:cfcfeccb959e 743
carlosperales95 37:bb15bea420a3 744
mglmx 34:c9ab2a987734 745 /**
mglmx 34:c9ab2a987734 746 *
carlosperales95 25:a42a1ed4d8e9 747 *Method to catch interrupts 0
carlosperales95 25:a42a1ed4d8e9 748 *
carlosperales95 24:1d71dd8778c4 749 **/
mglmx 33:24ce12dec157 750 void on_int0_change(){
carlosperales95 25:a42a1ed4d8e9 751
mglmx 33:24ce12dec157 752 wait_us(2000);
mglmx 33:24ce12dec157 753 int sensor_data = mcp->_read(INTCAPA);
mglmx 34:c9ab2a987734 754 int sensor = get_sensor(sensor_data,0);
mglmx 55:aa3baa01f43d 755 //lcd.cls();
mglmx 55:aa3baa01f43d 756 //lcd.printf("int0 0x%x \n Sensor: %d",sensor_data,sensor);
mglmx 41:4fa6aa29d1ed 757
mglmx 35:cfcfeccb959e 758 update_train_pos(sensor);
mglmx 16:2a2da0e67793 759 }
mglmx 16:2a2da0e67793 760
mglmx 35:cfcfeccb959e 761
carlosperales95 24:1d71dd8778c4 762 /**
carlosperales95 24:1d71dd8778c4 763 *
carlosperales95 25:a42a1ed4d8e9 764 *Method to catch interrupts 1
carlosperales95 25:a42a1ed4d8e9 765 *
carlosperales95 24:1d71dd8778c4 766 **/
mglmx 33:24ce12dec157 767 void on_int1_change(){
carlosperales95 25:a42a1ed4d8e9 768
mglmx 33:24ce12dec157 769 wait_us(2000);
mglmx 33:24ce12dec157 770 int sensor_data = mcp->_read(INTCAPB);
mglmx 34:c9ab2a987734 771 int sensor = get_sensor(sensor_data,1);
mglmx 55:aa3baa01f43d 772 //lcd.cls();
mglmx 55:aa3baa01f43d 773 //lcd.printf("int1 0x%x \n Sensor: %d",sensor_data,sensor);
mglmx 35:cfcfeccb959e 774
mglmx 35:cfcfeccb959e 775 update_train_pos(sensor);
mglmx 16:2a2da0e67793 776 }
mglmx 26:5c966a0a3e8e 777
carlosperales95 45:d589318238bf 778 /**
carlosperales95 45:d589318238bf 779 *
carlosperales95 47:e992a129ef44 780 *Clear current interrupts
carlosperales95 45:d589318238bf 781 *
carlosperales95 45:d589318238bf 782 **/
carlosperales95 47:e992a129ef44 783 void init() {
mglmx 26:5c966a0a3e8e 784
mglmx 33:24ce12dec157 785 mcp->_read(GPIOA);
mglmx 33:24ce12dec157 786 mcp->_read(GPIOB); // Register callbacks
mglmx 33:24ce12dec157 787 int0.fall(&on_int0_change);
mglmx 33:24ce12dec157 788 int1.fall(&on_int1_change); // Enable interrupts on MCP
mglmx 33:24ce12dec157 789 mcp->_write(GPINTENA, (unsigned char )0xff);
mglmx 33:24ce12dec157 790 mcp->_write(GPINTENB, (unsigned char )0xff); // Ready to go!
mglmx 33:24ce12dec157 791 }
mglmx 33:24ce12dec157 792
mglmx 33:24ce12dec157 793
carlosperales95 24:1d71dd8778c4 794 /**
carlosperales95 24:1d71dd8778c4 795 *
carlosperales95 25:a42a1ed4d8e9 796 *Method to send DCC commands to train and switches.
carlosperales95 25:a42a1ed4d8e9 797 *
carlosperales95 25:a42a1ed4d8e9 798 *@address - (HEX)Address where the commands will be sent
carlosperales95 25:a42a1ed4d8e9 799 *@inst - (HEX)Number of instruction that will be commanded
carlosperales95 25:a42a1ed4d8e9 800 *@repeat_count - Number of times the command will be sent
carlosperales95 25:a42a1ed4d8e9 801 *
carlosperales95 24:1d71dd8778c4 802 **/
carlosperales95 45:d589318238bf 803 void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count){
carlosperales95 45:d589318238bf 804
mglmx 1:0ab26889af9b 805 unsigned __int64 command = 0x0000000000000000; // __int64 is the 64-bit integer type
mglmx 1:0ab26889af9b 806 unsigned __int64 temp_command = 0x0000000000000000;
mglmx 1:0ab26889af9b 807 unsigned __int64 prefix = 0x3FFF; // 14 "1" bits needed at start
mglmx 1:0ab26889af9b 808 unsigned int error = 0x00; //error byte
carlosperales95 24:1d71dd8778c4 809
mglmx 1:0ab26889af9b 810 //calculate error detection byte with xor
mglmx 1:0ab26889af9b 811 error = address ^ inst;
carlosperales95 24:1d71dd8778c4 812
mglmx 1:0ab26889af9b 813 //combine packet bits in basic DCC format
mglmx 1:0ab26889af9b 814 command = (prefix<<28)|(address<<19)|(inst<<10)|((error)<<1)|0x01;
mglmx 1:0ab26889af9b 815 //printf("\n\r %llx \n\r",command);
carlosperales95 24:1d71dd8778c4 816
mglmx 1:0ab26889af9b 817 int i=0;
carlosperales95 24:1d71dd8778c4 818 //repeat DCC command lots of times
mglmx 1:0ab26889af9b 819 while(i < repeat_count) {
carlosperales95 24:1d71dd8778c4 820
mglmx 1:0ab26889af9b 821 temp_command = command;
carlosperales95 24:1d71dd8778c4 822 //loops through packet bits encoding and sending out digital pulses for a DCC command
mglmx 1:0ab26889af9b 823 for (int j=0; j<64; j++) {
carlosperales95 24:1d71dd8778c4 824
carlosperales95 24:1d71dd8778c4 825 if((temp_command&0x8000000000000000)==0) {
carlosperales95 24:1d71dd8778c4 826 //test packet bit
mglmx 1:0ab26889af9b 827 //send data for a "0" bit
mglmx 1:0ab26889af9b 828 Track=0;
mglmx 1:0ab26889af9b 829 wait_us(100);
mglmx 1:0ab26889af9b 830 Track=1;
mglmx 1:0ab26889af9b 831 wait_us(100);
mglmx 1:0ab26889af9b 832 //printf("0011");
carlosperales95 24:1d71dd8778c4 833 }else{
carlosperales95 24:1d71dd8778c4 834
mglmx 1:0ab26889af9b 835 //send data for a "1"bit
mglmx 1:0ab26889af9b 836 Track=0;
mglmx 1:0ab26889af9b 837 wait_us(58);
mglmx 1:0ab26889af9b 838 Track=1;
mglmx 1:0ab26889af9b 839 wait_us(58);
mglmx 1:0ab26889af9b 840 //printf("01");
mglmx 1:0ab26889af9b 841 }
mglmx 1:0ab26889af9b 842 // next bit in packet
mglmx 1:0ab26889af9b 843 temp_command = temp_command<<1;
mglmx 1:0ab26889af9b 844 }
mglmx 1:0ab26889af9b 845 i++;
mglmx 0:4d06a6a8e785 846 }
mglmx 0:4d06a6a8e785 847 }
carlosperales95 11:021210c59a95 848
carlosperales95 24:1d71dd8778c4 849
carlosperales95 24:1d71dd8778c4 850 /**
carlosperales95 24:1d71dd8778c4 851 *
carlosperales95 25:a42a1ed4d8e9 852 *Method to flip the switches
carlosperales95 25:a42a1ed4d8e9 853 *
carlosperales95 25:a42a1ed4d8e9 854 *@switchId - (1-4)The ID of the switch we want to flip
carlosperales95 25:a42a1ed4d8e9 855 *@times - The number of times we want to send the command
mglmx 30:293ee760d357 856 *@activate - True if the switch is going to be activated. False if it needs to go back to rest position.
carlosperales95 25:a42a1ed4d8e9 857 *
carlosperales95 24:1d71dd8778c4 858 **/
mglmx 55:aa3baa01f43d 859 void flip_switch(int switchId, int times, bool activate=true){
mglmx 21:e6f1649add39 860
carlosperales95 25:a42a1ed4d8e9 861 unsigned int SWBflip = SWBidle; //IDLE - Flip last activated SW.
mglmx 21:e6f1649add39 862
mglmx 21:e6f1649add39 863 switch(switchId){
carlosperales95 45:d589318238bf 864
mglmx 21:e6f1649add39 865 case 1:
carlosperales95 25:a42a1ed4d8e9 866 SWBflip = SWBflip_1; //FLIP SW1
mglmx 21:e6f1649add39 867 break;
carlosperales95 45:d589318238bf 868
mglmx 21:e6f1649add39 869 case 2:
carlosperales95 25:a42a1ed4d8e9 870 SWBflip = SWBflip_2; //FLIP SW2
mglmx 21:e6f1649add39 871 break;
carlosperales95 45:d589318238bf 872
mglmx 21:e6f1649add39 873 case 3:
carlosperales95 25:a42a1ed4d8e9 874 SWBflip = SWBflip_3; //FLIP SW3
mglmx 21:e6f1649add39 875 break;
carlosperales95 45:d589318238bf 876
mglmx 21:e6f1649add39 877 case 4:
carlosperales95 25:a42a1ed4d8e9 878 SWBflip = SWBflip_4; //FLIP SW4
mglmx 21:e6f1649add39 879 break;
carlosperales95 45:d589318238bf 880
mglmx 21:e6f1649add39 881 default:
carlosperales95 24:1d71dd8778c4 882 break;
mglmx 21:e6f1649add39 883 }
carlosperales95 11:021210c59a95 884
mglmx 21:e6f1649add39 885 //Security measure not to burn the switch.
mglmx 30:293ee760d357 886 if(times <=5){
carlosperales95 45:d589318238bf 887
mglmx 30:293ee760d357 888 DCC_send_command(SWBaddress,SWBflip,times); //Activating switch
mglmx 30:293ee760d357 889 if(!activate){
carlosperales95 45:d589318238bf 890
mglmx 30:293ee760d357 891 DCC_send_command(SWBaddress,SWBidle,times); //Sending IDLE to flip back.
mglmx 30:293ee760d357 892 }
mglmx 30:293ee760d357 893 }
mglmx 21:e6f1649add39 894 }
mglmx 21:e6f1649add39 895
mglmx 22:e4153ca757dd 896
carlosperales95 24:1d71dd8778c4 897 /**
carlosperales95 24:1d71dd8778c4 898 *
carlosperales95 29:559eb2164488 899 *Checks if any of the switches of the box has been activated.
carlosperales95 29:559eb2164488 900 *Calls necessary function and displays LCD text.
carlosperales95 25:a42a1ed4d8e9 901 *
carlosperales95 24:1d71dd8778c4 902 **/
mglmx 21:e6f1649add39 903 void checkSwitch(){
carlosperales95 24:1d71dd8778c4 904
mglmx 22:e4153ca757dd 905 if(switch1 == 1){
carlosperales95 24:1d71dd8778c4 906
mglmx 22:e4153ca757dd 907 lcd.cls();
mglmx 22:e4153ca757dd 908 lcd.printf("Switch 1 ON - SW1");
mglmx 55:aa3baa01f43d 909 flip_switch(1,5);
mglmx 22:e4153ca757dd 910 }else if(switch2 == 1){
carlosperales95 24:1d71dd8778c4 911
carlosperales95 24:1d71dd8778c4 912 lcd.cls();
carlosperales95 24:1d71dd8778c4 913 lcd.printf("Switch 2 ON - SW2");
mglmx 55:aa3baa01f43d 914 flip_switch(2,5);
carlosperales95 24:1d71dd8778c4 915 }else if(switch3 == 0){
carlosperales95 24:1d71dd8778c4 916
carlosperales95 24:1d71dd8778c4 917 lcd.cls();
carlosperales95 24:1d71dd8778c4 918 lcd.printf("Switch 3 ON - SW3");
mglmx 55:aa3baa01f43d 919 flip_switch(3,5);
carlosperales95 24:1d71dd8778c4 920 }else if(switch4 == 0){
carlosperales95 24:1d71dd8778c4 921
carlosperales95 24:1d71dd8778c4 922 lcd.cls();
carlosperales95 24:1d71dd8778c4 923 lcd.printf("Switch 4 ON - IDLE");
mglmx 55:aa3baa01f43d 924 flip_switch(0,5);
carlosperales95 24:1d71dd8778c4 925 }
mglmx 22:e4153ca757dd 926 }
mglmx 22:e4153ca757dd 927
carlosperales95 45:d589318238bf 928
carlosperales95 45:d589318238bf 929 /**
carlosperales95 45:d589318238bf 930 *
carlosperales95 45:d589318238bf 931 *
carlosperales95 45:d589318238bf 932 *
carlosperales95 45:d589318238bf 933 **/
mglmx 43:346a1f4144cd 934 void send_command(){
carlosperales95 45:d589318238bf 935
mglmx 43:346a1f4144cd 936 if(DR_run){
carlosperales95 45:d589318238bf 937
mglmx 43:346a1f4144cd 938 DCC_send_command(DCCaddressDR,DCCinst_forward,1); // Forward half speed train addres DARK-RED
mglmx 43:346a1f4144cd 939 }else{
carlosperales95 45:d589318238bf 940
mglmx 43:346a1f4144cd 941 DCC_send_command(DCCaddressDR,DCCinst_stop,400);
mglmx 43:346a1f4144cd 942 }
mglmx 43:346a1f4144cd 943
mglmx 43:346a1f4144cd 944 if(LR_run){
carlosperales95 45:d589318238bf 945
mglmx 52:c08495446f87 946 DCC_send_command(DCCaddressLR,DCCinst_forward_fast,1); // Forward half speed train addres DARK-RED
mglmx 43:346a1f4144cd 947 }else{
carlosperales95 45:d589318238bf 948
mglmx 43:346a1f4144cd 949 DCC_send_command(DCCaddressLR,DCCinst_stop,400);
mglmx 43:346a1f4144cd 950 }
mglmx 43:346a1f4144cd 951 }
carlosperales95 24:1d71dd8778c4 952
mglmx 55:aa3baa01f43d 953
mglmx 55:aa3baa01f43d 954 /**
mglmx 55:aa3baa01f43d 955 * Returns a sensor number depending on how many times switch3 flips.
mglmx 55:aa3baa01f43d 956 * When pressing switch4 it confirms the switch
mglmx 55:aa3baa01f43d 957 * Init_sensor is the value where we start counting.
mglmx 55:aa3baa01f43d 958 * string train is the name of the train that will be prited with the sensor
mglmx 55:aa3baa01f43d 959 */
mglmx 55:aa3baa01f43d 960 int select_sensor(int init_sensor, string train){
mglmx 55:aa3baa01f43d 961
mglmx 55:aa3baa01f43d 962 lcd.cls();
mglmx 55:aa3baa01f43d 963 lcd.printf("%s SENSOR",train);
mglmx 55:aa3baa01f43d 964
mglmx 55:aa3baa01f43d 965 int sensor = init_sensor;
mglmx 52:c08495446f87 966 bool changed = false;
mglmx 52:c08495446f87 967 bool exit = false;
mglmx 52:c08495446f87 968
mglmx 52:c08495446f87 969 while(!exit){
mglmx 52:c08495446f87 970 if(switch3 == 0){
mglmx 52:c08495446f87 971 if(changed){
mglmx 52:c08495446f87 972 sensor++;
mglmx 52:c08495446f87 973 sensor=sensor%15; //Only sensors from 0 to 15.
mglmx 52:c08495446f87 974 changed=false;
mglmx 55:aa3baa01f43d 975 lcd.cls();
mglmx 55:aa3baa01f43d 976 lcd.printf("%s: D%d",train,sensor);
mglmx 52:c08495446f87 977 }
mglmx 52:c08495446f87 978
mglmx 52:c08495446f87 979 }else{
mglmx 52:c08495446f87 980 changed = true;
mglmx 55:aa3baa01f43d 981 wait(0.2);
mglmx 52:c08495446f87 982 }
mglmx 52:c08495446f87 983
mglmx 52:c08495446f87 984 if(switch4 == 0){
mglmx 52:c08495446f87 985 exit = true;
mglmx 55:aa3baa01f43d 986 wait(0.2);
mglmx 52:c08495446f87 987 }
mglmx 52:c08495446f87 988 }
mglmx 52:c08495446f87 989
mglmx 52:c08495446f87 990 return sensor;
mglmx 52:c08495446f87 991 }
carlosperales95 45:d589318238bf 992
mglmx 55:aa3baa01f43d 993 /**
mglmx 55:aa3baa01f43d 994 * Returns a boolean representing the direction. Everytimew switch3 is 0 it changes the direction.
mglmx 55:aa3baa01f43d 995 * When switch4 is 0 the selection is confirmed.
mglmx 55:aa3baa01f43d 996 * Init_going_cw is the initial direction.
mglmx 55:aa3baa01f43d 997 * Train is the string with the name of the train that will be printed next to the direction
mglmx 55:aa3baa01f43d 998 */
mglmx 55:aa3baa01f43d 999 bool select_direction(bool init_going_cw,string train){
mglmx 55:aa3baa01f43d 1000
mglmx 55:aa3baa01f43d 1001 lcd.cls();
mglmx 55:aa3baa01f43d 1002 lcd.printf("%s DIRECTION",train);
mglmx 55:aa3baa01f43d 1003 bool exit = false;
mglmx 55:aa3baa01f43d 1004 bool going_cw = init_going_cw;
mglmx 55:aa3baa01f43d 1005 bool changed = false;
mglmx 55:aa3baa01f43d 1006
mglmx 55:aa3baa01f43d 1007 while(!exit){
mglmx 55:aa3baa01f43d 1008 if(switch3 == 0){
mglmx 55:aa3baa01f43d 1009 if(changed){
mglmx 55:aa3baa01f43d 1010 going_cw = !going_cw;
mglmx 55:aa3baa01f43d 1011 changed = false;
mglmx 55:aa3baa01f43d 1012 string dir;
mglmx 55:aa3baa01f43d 1013 if(going_cw){
mglmx 55:aa3baa01f43d 1014 dir = "cw";
mglmx 55:aa3baa01f43d 1015 }else{
mglmx 55:aa3baa01f43d 1016 dir = "ccw";
mglmx 55:aa3baa01f43d 1017 }
mglmx 55:aa3baa01f43d 1018 lcd.cls();
mglmx 55:aa3baa01f43d 1019 lcd.printf("%s: %s",train,dir);
mglmx 55:aa3baa01f43d 1020 }
mglmx 55:aa3baa01f43d 1021 }else{
mglmx 55:aa3baa01f43d 1022 changed = true;
mglmx 55:aa3baa01f43d 1023 wait(0.2);
mglmx 55:aa3baa01f43d 1024 }
mglmx 55:aa3baa01f43d 1025
mglmx 55:aa3baa01f43d 1026 if(switch4 == 0){
mglmx 55:aa3baa01f43d 1027 exit = true;
mglmx 55:aa3baa01f43d 1028 wait(0.2);
mglmx 55:aa3baa01f43d 1029 }
mglmx 55:aa3baa01f43d 1030 }
mglmx 55:aa3baa01f43d 1031
mglmx 55:aa3baa01f43d 1032 return going_cw;
mglmx 55:aa3baa01f43d 1033 }
mglmx 55:aa3baa01f43d 1034
carlosperales95 11:021210c59a95 1035 //**************** MAIN PROGRAM FOR DENVER TRAIN ****************//
carlosperales95 11:021210c59a95 1036
carlosperales95 45:d589318238bf 1037
mglmx 1:0ab26889af9b 1038 int main()
mglmx 1:0ab26889af9b 1039 {
carlosperales95 25:a42a1ed4d8e9 1040 //RISE FOR INTERRUPTS?? NOT WORKING ATM
carlosperales95 25:a42a1ed4d8e9 1041 //int0.rise(&interrupt0);
carlosperales95 25:a42a1ed4d8e9 1042 //int1.rise(&interrupt1);
carlosperales95 25:a42a1ed4d8e9 1043
carlosperales95 25:a42a1ed4d8e9 1044 //Read and display potentiometer
carlosperales95 25:a42a1ed4d8e9 1045 //float f = pot.read();
carlosperales95 25:a42a1ed4d8e9 1046 //float vin = f * 3.3;
carlosperales95 25:a42a1ed4d8e9 1047 //lcd.printf("vin: %.4f",vin);
carlosperales95 25:a42a1ed4d8e9 1048
carlosperales95 25:a42a1ed4d8e9 1049 //0xFFFC //1111111111111100
carlosperales95 25:a42a1ed4d8e9 1050
carlosperales95 50:ee4398ee44be 1051 enable = 1;
carlosperales95 25:a42a1ed4d8e9 1052
carlosperales95 25:a42a1ed4d8e9 1053 //Led routine to start main program
mglmx 2:f580707c44fa 1054 led1 = 1;
mglmx 22:e4153ca757dd 1055 wait(0.2);
mglmx 2:f580707c44fa 1056 led1 = 0;
mglmx 22:e4153ca757dd 1057 wait(0.2);
mglmx 2:f580707c44fa 1058 led1 = 1;
mglmx 16:2a2da0e67793 1059
mglmx 55:aa3baa01f43d 1060 init_positions();
mglmx 55:aa3baa01f43d 1061
carlosperales95 25:a42a1ed4d8e9 1062 initialize_mcp(); //mcp initialization for interrupts before train running
mglmx 33:24ce12dec157 1063 init();
mglmx 55:aa3baa01f43d 1064
mglmx 55:aa3baa01f43d 1065
mglmx 55:aa3baa01f43d 1066
mglmx 55:aa3baa01f43d 1067 int DR_init_sensor = select_sensor(D9,"DR");
mglmx 55:aa3baa01f43d 1068 bool DR_init_dir = select_direction(false,"DR");
mglmx 55:aa3baa01f43d 1069
mglmx 55:aa3baa01f43d 1070 wait(0.5);
mglmx 55:aa3baa01f43d 1071
mglmx 55:aa3baa01f43d 1072 int LR_init_sensor = select_sensor(D9,"LR");
mglmx 55:aa3baa01f43d 1073 bool LR_init_dir = select_direction(true,"LR");
mglmx 55:aa3baa01f43d 1074
mglmx 55:aa3baa01f43d 1075 DR_train.set_position(DR_init_sensor);
mglmx 55:aa3baa01f43d 1076 DR_train.set_goes_cw(DR_init_dir);
mglmx 55:aa3baa01f43d 1077
mglmx 55:aa3baa01f43d 1078 LR_train.set_position(LR_init_sensor);
mglmx 55:aa3baa01f43d 1079 LR_train.set_goes_cw(LR_init_dir);
mglmx 55:aa3baa01f43d 1080
mglmx 16:2a2da0e67793 1081
mglmx 55:aa3baa01f43d 1082 string DR_print_dir, LR_print_dir;
mglmx 55:aa3baa01f43d 1083
mglmx 55:aa3baa01f43d 1084 if(DR_train.goes_cw()){
mglmx 55:aa3baa01f43d 1085 DR_print_dir = "cw";
mglmx 55:aa3baa01f43d 1086 }else{
mglmx 55:aa3baa01f43d 1087 DR_print_dir = "ccw";
mglmx 55:aa3baa01f43d 1088 }
mglmx 55:aa3baa01f43d 1089
mglmx 55:aa3baa01f43d 1090 if(LR_train.goes_cw()){
mglmx 55:aa3baa01f43d 1091 LR_print_dir = "cw";
mglmx 55:aa3baa01f43d 1092 }else{
mglmx 55:aa3baa01f43d 1093 LR_print_dir = "ccw";
mglmx 55:aa3baa01f43d 1094 }
mglmx 55:aa3baa01f43d 1095
mglmx 55:aa3baa01f43d 1096 lcd.cls();
mglmx 55:aa3baa01f43d 1097 lcd.printf("DR(%d)%s \n LR(%d)%s",DR_train.get_position_number(),DR_print_dir,LR_train.get_position_number(),LR_print_dir);
mglmx 55:aa3baa01f43d 1098
mglmx 55:aa3baa01f43d 1099 wait(2);
mglmx 55:aa3baa01f43d 1100
mglmx 43:346a1f4144cd 1101
carlosperales95 25:a42a1ed4d8e9 1102 //Train light routine to start running
mglmx 43:346a1f4144cd 1103 /*
mglmx 32:e5b732fb8e65 1104 DCC_send_command(DCCaddressDR,DCC_func_lighton,200); // turn light on full
mglmx 52:c08495446f87 1105 DCC_send_command(DCCaddressDR,DCC_func_dimlight,400); // dim light
mglmx 52:c08495446f87 1106 DCC_send_command(DCCaddressDR,DCC_func_lighton,200); // light full again
mglmx 43:346a1f4144cd 1107 */
mglmx 22:e4153ca757dd 1108
carlosperales95 25:a42a1ed4d8e9 1109 //LED3 Shows start of route + LCD notif
mglmx 22:e4153ca757dd 1110 led3 = 1; // Entering the while
mglmx 22:e4153ca757dd 1111 lcd.cls();
mglmx 22:e4153ca757dd 1112 lcd.printf("Ready to start");
mglmx 55:aa3baa01f43d 1113 wait(1);
carlosperales95 19:ff21ba3a4dc5 1114
carlosperales95 14:7bb998edd819 1115 //Demo for stopping at the station
mglmx 1:0ab26889af9b 1116 while(1) {
mglmx 4:50879dfb82d5 1117
mglmx 55:aa3baa01f43d 1118 checkSwitch(); //Checks for switch commands everytime.
mglmx 55:aa3baa01f43d 1119 check_position();
carlosperales95 25:a42a1ed4d8e9 1120
mglmx 33:24ce12dec157 1121 if(1==0){
mglmx 33:24ce12dec157 1122 //if(station == 1){ //If train is on the sensor at the middle of the station it stops and displays LCD text.
carlosperales95 14:7bb998edd819 1123
mglmx 4:50879dfb82d5 1124 lcd.cls();
carlosperales95 29:559eb2164488 1125 lcd.printf("All aboard\n mind the gap");
mglmx 32:e5b732fb8e65 1126 DCC_send_command(DCCaddressDR,DCCinst_stop,400);
carlosperales95 29:559eb2164488 1127 lcd.cls();
mglmx 4:50879dfb82d5 1128
mglmx 22:e4153ca757dd 1129 }else{
mglmx 43:346a1f4144cd 1130 send_command();
mglmx 55:aa3baa01f43d 1131
carlosperales95 25:a42a1ed4d8e9 1132 }
mglmx 1:0ab26889af9b 1133 }
mglmx 3:fe7010b693a0 1134 }