Denver / Mbed 2 deprecated denver_train_proj

Denver trai project

Dependencies:   mbed TextLCD

main.cpp@70:594fe5c1ad5c, 2018-06-27 (annotated)

Committer:
carlosperales95
Date:
Wed Jun 27 09:22:45 2018 +0000
Revision:
70:594fe5c1ad5c
Parent:
69:609c69e91b4e
Child:
71:9a303d90c9b8
Logger functionality deleted :(

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 62:6fc0c683cb02 7 #include <fstream>
mglmx 62:6fc0c683cb02 8 #include <sstream>
mglmx 35:cfcfeccb959e 9
mglmx 35:cfcfeccb959e 10 using namespace std;
mglmx 18:aa43bb62e60f 11
carlosperales95 65:8fd4046c6540 12
carlosperales95 7:e2b8461d4f05 13 /******PINS AND DECLARATIONS*******/
carlosperales95 7:e2b8461d4f05 14
carlosperales95 25:a42a1ed4d8e9 15 //------PINS
mglmx 21:e6f1649add39 16
mglmx 21:e6f1649add39 17 //SWITCHES p5 - p8
mglmx 21:e6f1649add39 18 DigitalIn switch1(p5);
mglmx 21:e6f1649add39 19 DigitalIn switch2(p6);
mglmx 21:e6f1649add39 20 DigitalIn switch3(p7);
mglmx 21:e6f1649add39 21 DigitalIn switch4(p8);
carlosperales95 7:e2b8461d4f05 22
carlosperales95 10:2088b1935a93 23 //RAIL SENSORS - INT0,INT1
carlosperales95 7:e2b8461d4f05 24 //INT0 - p9
mglmx 18:aa43bb62e60f 25 InterruptIn int0(p9);
carlosperales95 7:e2b8461d4f05 26 //INT1 - p10
mglmx 18:aa43bb62e60f 27 InterruptIn int1(p10);
carlosperales95 7:e2b8461d4f05 28
carlosperales95 7:e2b8461d4f05 29 ///p11
carlosperales95 7:e2b8461d4f05 30 ///p12
carlosperales95 12:e914ca5cd44b 31
carlosperales95 7:e2b8461d4f05 32 //M0 - p13
mglmx 35:cfcfeccb959e 33 DigitalIn d21stat(p13); //Sensor right of the station
carlosperales95 7:e2b8461d4f05 34 //M1 - p14
mglmx 35:cfcfeccb959e 35 DigitalIn d22stat(p14); //Sensor left of the station
carlosperales95 7:e2b8461d4f05 36 //M2 - p15
carlosperales95 29:559eb2164488 37 DigitalIn station(p15); //Sensor in the middle of the station
carlosperales95 12:e914ca5cd44b 38
carlosperales95 7:e2b8461d4f05 39 //p16
carlosperales95 50:ee4398ee44be 40 //ENABLE - p17
carlosperales95 50:ee4398ee44be 41 DigitalOut enable(p17);
carlosperales95 12:e914ca5cd44b 42
carlosperales95 11:021210c59a95 43 //BUZZER - p18
carlosperales95 11:021210c59a95 44 DigitalOut buzz(p18); // buzz=0 doesn't beep, buzz=1 beeps
carlosperales95 7:e2b8461d4f05 45
carlosperales95 7:e2b8461d4f05 46 //POTENTIOMETER - p19
carlosperales95 13:dbf1ead12cee 47 AnalogIn pot(p19); //Gives float value pot.read(). Convert analog input to V with f*3.3
carlosperales95 7:e2b8461d4f05 48
carlosperales95 7:e2b8461d4f05 49 //DAT - p20
mglmx 3:fe7010b693a0 50 DigitalOut Track(p20); //Digital output bit used to drive track power via H-bridge
carlosperales95 7:e2b8461d4f05 51
carlosperales95 7:e2b8461d4f05 52 //LCD SCREEN - p21, p22, p23, p24, p25, p26
carlosperales95 11:021210c59a95 53 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 54
carlosperales95 7:e2b8461d4f05 55 ///p27
mglmx 62:6fc0c683cb02 56 ///p28
mglmx 33:24ce12dec157 57 I2C i2c(p28,p27);
carlosperales95 7:e2b8461d4f05 58
carlosperales95 7:e2b8461d4f05 59 //LED1 - p29
mglmx 3:fe7010b693a0 60 DigitalOut redled(p29);
carlosperales95 7:e2b8461d4f05 61 //LED2 - p30
mglmx 3:fe7010b693a0 62 DigitalOut greenled(p30);
carlosperales95 7:e2b8461d4f05 63
carlosperales95 24:1d71dd8778c4 64 //MBED LEDS
mglmx 18:aa43bb62e60f 65 DigitalOut led1(LED1);
mglmx 18:aa43bb62e60f 66 DigitalOut led2(LED2);
mglmx 18:aa43bb62e60f 67 DigitalOut led3(LED3);
carlosperales95 69:609c69e91b4e 68 DigitalOut led4(LED4);
mglmx 16:2a2da0e67793 69
mglmx 16:2a2da0e67793 70 //MCP
mglmx 16:2a2da0e67793 71 MCP23017 *mcp;
carlosperales95 12:e914ca5cd44b 72
carlosperales95 65:8fd4046c6540 73 //------
carlosperales95 65:8fd4046c6540 74
carlosperales95 24:1d71dd8778c4 75
carlosperales95 25:a42a1ed4d8e9 76 //------GLOBAL VARS
carlosperales95 25:a42a1ed4d8e9 77
carlosperales95 38:b9aba3715682 78 //......SENSOR POSITION VARS
carlosperales95 38:b9aba3715682 79
carlosperales95 45:d589318238bf 80 //Definition of D sensors, will be interpreted as ints for the program's logic
mglmx 34:c9ab2a987734 81 #define D0 0
mglmx 34:c9ab2a987734 82 #define D1 1
mglmx 34:c9ab2a987734 83 #define D2 2
mglmx 34:c9ab2a987734 84 #define D3 3
mglmx 34:c9ab2a987734 85 #define D4 4
mglmx 34:c9ab2a987734 86 #define D5 5
mglmx 34:c9ab2a987734 87 #define D6 6
mglmx 34:c9ab2a987734 88 #define D7 7
mglmx 34:c9ab2a987734 89 #define D8 8
mglmx 34:c9ab2a987734 90 #define D9 9
mglmx 34:c9ab2a987734 91 #define D10 10
mglmx 34:c9ab2a987734 92 #define D11 11
mglmx 34:c9ab2a987734 93 #define D12 12
mglmx 34:c9ab2a987734 94 #define D13 13
mglmx 34:c9ab2a987734 95 #define D21 14
mglmx 34:c9ab2a987734 96 #define D22 15
mglmx 34:c9ab2a987734 97
carlosperales95 65:8fd4046c6540 98 //Definition of the different train speeds, will be interpreted as ints for the program's logic
mglmx 57:ee5da8a011e0 99 #define STOP 0
mglmx 57:ee5da8a011e0 100 #define SLOW 1
mglmx 57:ee5da8a011e0 101 #define MEDIUM 2
mglmx 57:ee5da8a011e0 102 #define FAST 3
mglmx 57:ee5da8a011e0 103 #define FULL 4
mglmx 57:ee5da8a011e0 104 #define R_MEDIUM 5
mglmx 55:aa3baa01f43d 105
mglmx 55:aa3baa01f43d 106
mglmx 62:6fc0c683cb02 107
carlosperales95 38:b9aba3715682 108 /**
carlosperales95 38:b9aba3715682 109 *
carlosperales95 38:b9aba3715682 110 *Position class.
carlosperales95 38:b9aba3715682 111 *
carlosperales95 38:b9aba3715682 112 *@position -
carlosperales95 38:b9aba3715682 113 *@previous_cw -
carlosperales95 38:b9aba3715682 114 *@previous_ccw -
carlosperales95 38:b9aba3715682 115 *
carlosperales95 38:b9aba3715682 116 *Position(int) -
carlosperales95 38:b9aba3715682 117 *
carlosperales95 38:b9aba3715682 118 *get_pos() -
carlosperales95 38:b9aba3715682 119 *get_prev_cw() -
mglmx 62:6fc0c683cb02 120 *get_ccw() -
carlosperales95 38:b9aba3715682 121 *add_prev_cw() -
mglmx 62:6fc0c683cb02 122 *add_ccw() -
carlosperales95 38:b9aba3715682 123 *
carlosperales95 38:b9aba3715682 124 **/
mglmx 35:cfcfeccb959e 125 class Position{
mglmx 35:cfcfeccb959e 126 private:
mglmx 35:cfcfeccb959e 127 int position;
mglmx 35:cfcfeccb959e 128 vector <int> previous_cw;
mglmx 35:cfcfeccb959e 129 vector <int> previous_ccw;
mglmx 35:cfcfeccb959e 130 public:
mglmx 35:cfcfeccb959e 131 Position(int p){
mglmx 35:cfcfeccb959e 132 position = p;
mglmx 35:cfcfeccb959e 133 }
mglmx 36:9428c72bdd58 134
mglmx 36:9428c72bdd58 135 int get_pos(){
mglmx 36:9428c72bdd58 136 return position;
mglmx 36:9428c72bdd58 137 }
mglmx 41:4fa6aa29d1ed 138
mglmx 41:4fa6aa29d1ed 139 vector<int> get_next_cw(){
mglmx 41:4fa6aa29d1ed 140 return previous_ccw;
mglmx 41:4fa6aa29d1ed 141 }
mglmx 41:4fa6aa29d1ed 142
mglmx 41:4fa6aa29d1ed 143 vector<int> get_next_ccw(){
mglmx 41:4fa6aa29d1ed 144 return previous_cw;
mglmx 41:4fa6aa29d1ed 145 }
carlosperales95 25:a42a1ed4d8e9 146
mglmx 35:cfcfeccb959e 147 vector <int> get_prev_cw(){
mglmx 35:cfcfeccb959e 148 return previous_cw;
mglmx 35:cfcfeccb959e 149 }
mglmx 35:cfcfeccb959e 150
mglmx 35:cfcfeccb959e 151 vector <int> get_prev_ccw(){
mglmx 35:cfcfeccb959e 152 return previous_ccw;
mglmx 35:cfcfeccb959e 153 }
mglmx 35:cfcfeccb959e 154
mglmx 35:cfcfeccb959e 155 void add_prev_cw(int pos){
mglmx 35:cfcfeccb959e 156 previous_cw.push_back(pos);
mglmx 35:cfcfeccb959e 157 };
mglmx 35:cfcfeccb959e 158
mglmx 35:cfcfeccb959e 159 void add_prev_ccw(int pos){
mglmx 35:cfcfeccb959e 160 previous_ccw.push_back(pos);
mglmx 35:cfcfeccb959e 161 };
mglmx 35:cfcfeccb959e 162 };
mglmx 35:cfcfeccb959e 163
carlosperales95 45:d589318238bf 164
mglmx 41:4fa6aa29d1ed 165 //Creating a vector with all the positions.
mglmx 41:4fa6aa29d1ed 166 vector<Position> positions;
mglmx 41:4fa6aa29d1ed 167
carlosperales95 45:d589318238bf 168
carlosperales95 45:d589318238bf 169 /**
carlosperales95 45:d589318238bf 170 *
mglmx 57:ee5da8a011e0 171 *Method to send DCC commands to train and switches.
mglmx 57:ee5da8a011e0 172 *
mglmx 57:ee5da8a011e0 173 *@address - (HEX)Address where the commands will be sent
mglmx 57:ee5da8a011e0 174 *@inst - (HEX)Number of instruction that will be commanded
mglmx 57:ee5da8a011e0 175 *@repeat_count - Number of times the command will be sent
mglmx 57:ee5da8a011e0 176 *
mglmx 57:ee5da8a011e0 177 **/
mglmx 57:ee5da8a011e0 178 void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count){
mglmx 57:ee5da8a011e0 179
mglmx 57:ee5da8a011e0 180 unsigned __int64 command = 0x0000000000000000; // __int64 is the 64-bit integer type
mglmx 57:ee5da8a011e0 181 unsigned __int64 temp_command = 0x0000000000000000;
mglmx 57:ee5da8a011e0 182 unsigned __int64 prefix = 0x3FFF; // 14 "1" bits needed at start
mglmx 57:ee5da8a011e0 183 unsigned int error = 0x00; //error byte
mglmx 57:ee5da8a011e0 184
mglmx 57:ee5da8a011e0 185 //calculate error detection byte with xor
mglmx 57:ee5da8a011e0 186 error = address ^ inst;
mglmx 57:ee5da8a011e0 187
mglmx 57:ee5da8a011e0 188 //combine packet bits in basic DCC format
mglmx 57:ee5da8a011e0 189 command = (prefix<<28)|(address<<19)|(inst<<10)|((error)<<1)|0x01;
mglmx 57:ee5da8a011e0 190 //printf("\n\r %llx \n\r",command);
mglmx 57:ee5da8a011e0 191
mglmx 57:ee5da8a011e0 192 int i=0;
mglmx 57:ee5da8a011e0 193 //repeat DCC command lots of times
mglmx 57:ee5da8a011e0 194 while(i < repeat_count) {
mglmx 57:ee5da8a011e0 195
mglmx 57:ee5da8a011e0 196 temp_command = command;
mglmx 57:ee5da8a011e0 197 //loops through packet bits encoding and sending out digital pulses for a DCC command
mglmx 57:ee5da8a011e0 198 for (int j=0; j<64; j++) {
mglmx 57:ee5da8a011e0 199
mglmx 57:ee5da8a011e0 200 if((temp_command&0x8000000000000000)==0) {
mglmx 57:ee5da8a011e0 201 //test packet bit
mglmx 57:ee5da8a011e0 202 //send data for a "0" bit
mglmx 57:ee5da8a011e0 203 Track=0;
mglmx 57:ee5da8a011e0 204 wait_us(100);
mglmx 57:ee5da8a011e0 205 Track=1;
mglmx 57:ee5da8a011e0 206 wait_us(100);
mglmx 57:ee5da8a011e0 207 //printf("0011");
mglmx 57:ee5da8a011e0 208 }else{
mglmx 57:ee5da8a011e0 209
mglmx 57:ee5da8a011e0 210 //send data for a "1"bit
mglmx 57:ee5da8a011e0 211 Track=0;
mglmx 57:ee5da8a011e0 212 wait_us(58);
mglmx 57:ee5da8a011e0 213 Track=1;
mglmx 57:ee5da8a011e0 214 wait_us(58);
mglmx 57:ee5da8a011e0 215 //printf("01");
mglmx 57:ee5da8a011e0 216 }
mglmx 57:ee5da8a011e0 217 // next bit in packet
mglmx 57:ee5da8a011e0 218 temp_command = temp_command<<1;
mglmx 57:ee5da8a011e0 219 }
mglmx 57:ee5da8a011e0 220 i++;
mglmx 57:ee5da8a011e0 221 }
mglmx 57:ee5da8a011e0 222 }
mglmx 57:ee5da8a011e0 223
mglmx 57:ee5da8a011e0 224 /**
mglmx 57:ee5da8a011e0 225 *Defining areas for train detection and collision logic.
mglmx 57:ee5da8a011e0 226 *area_A_arr/area_B_arr - Arrays that hold the Dsensors for each area, used to initialize the vectors.
mglmx 57:ee5da8a011e0 227 *area_A/area_B - Vectors that hold the different sensors of the corresponding areas of the track.
mglmx 57:ee5da8a011e0 228 **/
mglmx 57:ee5da8a011e0 229 int area_A_arr[] = {D21,D2,D22,D1,D0,D13,D12};
mglmx 57:ee5da8a011e0 230 int area_B_arr[] = {D6,D7,D8};
mglmx 57:ee5da8a011e0 231
mglmx 57:ee5da8a011e0 232 const vector<int> area_A(area_A_arr,area_A_arr + sizeof(area_A_arr) / sizeof(int));
mglmx 57:ee5da8a011e0 233 const vector<int> area_B(area_B_arr,area_B_arr + sizeof(area_B_arr) / sizeof(int));
mglmx 57:ee5da8a011e0 234
mglmx 57:ee5da8a011e0 235
mglmx 62:6fc0c683cb02 236
mglmx 57:ee5da8a011e0 237 /**
mglmx 57:ee5da8a011e0 238 *
carlosperales95 45:d589318238bf 239 *Train class.
carlosperales95 45:d589318238bf 240 *
carlosperales95 45:d589318238bf 241 *@position -
carlosperales95 45:d589318238bf 242 *@going_cw -
carlosperales95 45:d589318238bf 243 *
carlosperales95 45:d589318238bf 244 *Train(int, bool) -
mglmx 62:6fc0c683cb02 245 *Train(bool) -
carlosperales95 58:b60db1092088 246 *
mglmx 62:6fc0c683cb02 247 *Vector get_next_sensors() -
carlosperales95 58:b60db1092088 248 *set_position(int) -
carlosperales95 58:b60db1092088 249 *set_goes_cw(bool) -
carlosperales95 45:d589318238bf 250 *Position get_position() -
carlosperales95 45:d589318238bf 251 *Int get_position_number() -
carlosperales95 45:d589318238bf 252 *Bool goes_cw() -
carlosperales95 45:d589318238bf 253 *
carlosperales95 45:d589318238bf 254 **/
mglmx 41:4fa6aa29d1ed 255 class Train{
carlosperales95 45:d589318238bf 256
mglmx 62:6fc0c683cb02 257 private:
mglmx 62:6fc0c683cb02 258
mglmx 57:ee5da8a011e0 259 unsigned int train_address; //stop the train
mglmx 41:4fa6aa29d1ed 260 Position *position;
mglmx 41:4fa6aa29d1ed 261 bool going_cw;
mglmx 57:ee5da8a011e0 262 int speed;
carlosperales95 45:d589318238bf 263
mglmx 41:4fa6aa29d1ed 264 public:
mglmx 41:4fa6aa29d1ed 265 Train(int pos, bool cw){
carlosperales95 45:d589318238bf 266
mglmx 41:4fa6aa29d1ed 267 position = &positions[pos];
mglmx 41:4fa6aa29d1ed 268 going_cw = cw;
mglmx 41:4fa6aa29d1ed 269 }
carlosperales95 45:d589318238bf 270
mglmx 57:ee5da8a011e0 271 /**
mglmx 62:6fc0c683cb02 272 * Contructor that takes the address of the train and the speed with default value MEDIUM.
mglmx 62:6fc0c683cb02 273 */
mglmx 62:6fc0c683cb02 274 Train(unsigned int address, int s=MEDIUM){
mglmx 57:ee5da8a011e0 275 train_address = address;
mglmx 57:ee5da8a011e0 276 speed = s;
mglmx 57:ee5da8a011e0 277 }
mglmx 62:6fc0c683cb02 278
mglmx 62:6fc0c683cb02 279 Train(bool cw){ going_cw = cw; }
mglmx 62:6fc0c683cb02 280
mglmx 41:4fa6aa29d1ed 281 vector<int> get_next_sensors(){
mglmx 41:4fa6aa29d1ed 282
mglmx 41:4fa6aa29d1ed 283 //Checking direction
mglmx 62:6fc0c683cb02 284 if(going_cw){
mglmx 62:6fc0c683cb02 285
mglmx 62:6fc0c683cb02 286 return position->get_next_cw();
mglmx 62:6fc0c683cb02 287 }else{
mglmx 62:6fc0c683cb02 288
mglmx 62:6fc0c683cb02 289 return position->get_next_ccw();
mglmx 62:6fc0c683cb02 290 }
mglmx 41:4fa6aa29d1ed 291 }
mglmx 41:4fa6aa29d1ed 292
mglmx 57:ee5da8a011e0 293 void set_speed(int s){
mglmx 57:ee5da8a011e0 294 speed = s;
mglmx 57:ee5da8a011e0 295 }
mglmx 57:ee5da8a011e0 296
mglmx 57:ee5da8a011e0 297 /**
mglmx 57:ee5da8a011e0 298 * Sends a DCC command to the train with the speed indicaed by the attribute speed
mglmx 57:ee5da8a011e0 299 * The number of times the command is sent can be indicated as an optional parameter. Default value is 1.
mglmx 57:ee5da8a011e0 300 */
mglmx 62:6fc0c683cb02 301 void run(int times=1){
mglmx 57:ee5da8a011e0 302
mglmx 57:ee5da8a011e0 303 const unsigned int DCCinst_forward_slow = 0x66; //forward slow speed (step 9)
mglmx 57:ee5da8a011e0 304 const unsigned int DCCinst_forward_medium = 0x68; //forward half speed
mglmx 57:ee5da8a011e0 305 const unsigned int DCCinst_forward_fast = 0x6C; //Forward fast speed (step 22)
mglmx 57:ee5da8a011e0 306 const unsigned int DCCinst_forward_full = 0x6F; //Forward full speed
mglmx 57:ee5da8a011e0 307 const unsigned int DCCinst_reverse_medium = 0x48; //reverse half speed
mglmx 57:ee5da8a011e0 308 const unsigned int DCCinst_stop = 0x50; //stop the train
mglmx 57:ee5da8a011e0 309
mglmx 57:ee5da8a011e0 310 switch(speed){
mglmx 62:6fc0c683cb02 311 case STOP:
mglmx 57:ee5da8a011e0 312 DCC_send_command(train_address, DCCinst_stop,times);
mglmx 57:ee5da8a011e0 313 break;
mglmx 57:ee5da8a011e0 314 case SLOW:
mglmx 57:ee5da8a011e0 315 DCC_send_command(train_address, DCCinst_forward_slow,times);
mglmx 57:ee5da8a011e0 316 break;
mglmx 57:ee5da8a011e0 317 case MEDIUM:
mglmx 57:ee5da8a011e0 318 DCC_send_command(train_address, DCCinst_forward_medium,times);
mglmx 57:ee5da8a011e0 319 break;
mglmx 57:ee5da8a011e0 320 case FAST:
mglmx 57:ee5da8a011e0 321 DCC_send_command(train_address, DCCinst_forward_fast,times);
mglmx 57:ee5da8a011e0 322 break;
mglmx 57:ee5da8a011e0 323 case FULL:
mglmx 57:ee5da8a011e0 324 DCC_send_command(train_address, DCCinst_forward_full,times);
mglmx 57:ee5da8a011e0 325 break;
mglmx 57:ee5da8a011e0 326 case R_MEDIUM:
mglmx 57:ee5da8a011e0 327 DCC_send_command(train_address, DCCinst_reverse_medium,times);
mglmx 57:ee5da8a011e0 328 break;
mglmx 57:ee5da8a011e0 329 }
mglmx 57:ee5da8a011e0 330 }
mglmx 57:ee5da8a011e0 331
mglmx 41:4fa6aa29d1ed 332 void set_position(int pos){
carlosperales95 45:d589318238bf 333
mglmx 41:4fa6aa29d1ed 334 position = &positions[pos]; //Taking the new position from the positions vector
mglmx 41:4fa6aa29d1ed 335 }
mglmx 41:4fa6aa29d1ed 336
mglmx 41:4fa6aa29d1ed 337 void set_goes_cw(bool cw){
carlosperales95 45:d589318238bf 338
mglmx 41:4fa6aa29d1ed 339 going_cw = cw;
mglmx 41:4fa6aa29d1ed 340 }
mglmx 41:4fa6aa29d1ed 341
mglmx 41:4fa6aa29d1ed 342 Position get_position(){
carlosperales95 45:d589318238bf 343
mglmx 41:4fa6aa29d1ed 344 return *position;
mglmx 41:4fa6aa29d1ed 345 }
mglmx 41:4fa6aa29d1ed 346
mglmx 41:4fa6aa29d1ed 347 int get_position_number(){
carlosperales95 45:d589318238bf 348
mglmx 41:4fa6aa29d1ed 349 return position->get_pos();
mglmx 41:4fa6aa29d1ed 350 }
mglmx 41:4fa6aa29d1ed 351
mglmx 41:4fa6aa29d1ed 352 bool goes_cw(){
carlosperales95 45:d589318238bf 353
mglmx 41:4fa6aa29d1ed 354 return going_cw;
mglmx 41:4fa6aa29d1ed 355 }
mglmx 57:ee5da8a011e0 356
mglmx 62:6fc0c683cb02 357
mglmx 57:ee5da8a011e0 358 /**
mglmx 57:ee5da8a011e0 359 *
mglmx 57:ee5da8a011e0 360 *Checks if the element exists within the vector.
mglmx 57:ee5da8a011e0 361 *
mglmx 57:ee5da8a011e0 362 *@v - The vector (of ints) the method will go through.
mglmx 57:ee5da8a011e0 363 *@element - The element the method will look for.
mglmx 57:ee5da8a011e0 364 *
mglmx 57:ee5da8a011e0 365 **/
mglmx 57:ee5da8a011e0 366 bool in_vector(vector<int>v,int element){
mglmx 57:ee5da8a011e0 367
mglmx 57:ee5da8a011e0 368 bool exist = false;
mglmx 57:ee5da8a011e0 369
mglmx 57:ee5da8a011e0 370 for(int i=0; i< v.size(); i++){
mglmx 57:ee5da8a011e0 371
mglmx 57:ee5da8a011e0 372 if(v[i] == element){
mglmx 57:ee5da8a011e0 373
mglmx 57:ee5da8a011e0 374 exist = true;
mglmx 57:ee5da8a011e0 375 }
mglmx 57:ee5da8a011e0 376 }
mglmx 57:ee5da8a011e0 377 return exist;
mglmx 57:ee5da8a011e0 378 }
mglmx 57:ee5da8a011e0 379
mglmx 57:ee5da8a011e0 380 bool is_in_A(){
mglmx 62:6fc0c683cb02 381 return in_vector(area_A,get_position_number());
mglmx 57:ee5da8a011e0 382
mglmx 57:ee5da8a011e0 383 }
mglmx 57:ee5da8a011e0 384
mglmx 57:ee5da8a011e0 385 bool is_in_B(){
mglmx 57:ee5da8a011e0 386
mglmx 62:6fc0c683cb02 387 return in_vector(area_B,get_position_number());
mglmx 57:ee5da8a011e0 388 }
mglmx 41:4fa6aa29d1ed 389 };
mglmx 41:4fa6aa29d1ed 390
carlosperales95 45:d589318238bf 391
carlosperales95 38:b9aba3715682 392 //Creation of all the positions. One for every sensor on the table - Position name(mapping)
mglmx 35:cfcfeccb959e 393 Position d0(D0);
mglmx 35:cfcfeccb959e 394 Position d1(D1);
mglmx 35:cfcfeccb959e 395 Position d2(D2);
mglmx 35:cfcfeccb959e 396 Position d3(D3);
mglmx 35:cfcfeccb959e 397 Position d4(D4);
mglmx 35:cfcfeccb959e 398 Position d5(D5);
mglmx 35:cfcfeccb959e 399 Position d6(D6);
mglmx 35:cfcfeccb959e 400 Position d7(D7);
mglmx 35:cfcfeccb959e 401 Position d8(D8);
mglmx 35:cfcfeccb959e 402 Position d9(D9);
mglmx 35:cfcfeccb959e 403 Position d10(D10);
mglmx 35:cfcfeccb959e 404 Position d11(D11);
mglmx 35:cfcfeccb959e 405 Position d12(D12);
mglmx 35:cfcfeccb959e 406 Position d13(D13);
mglmx 35:cfcfeccb959e 407 Position d21(D21);
mglmx 35:cfcfeccb959e 408 Position d22(D22);
mglmx 35:cfcfeccb959e 409
mglmx 43:346a1f4144cd 410
carlosperales95 25:a42a1ed4d8e9 411 //01DCSSSS for speed, D is direction (fwd=1 and rev=0), C is speed(SSSSC) LSB
carlosperales95 25:a42a1ed4d8e9 412 const unsigned int DCCinst_forward = 0x68; //forward half speed
mglmx 51:badef9fc202f 413 const unsigned int DCCinst_forward_slow = 0x66; //forward slow speed (step 9)
mglmx 51:badef9fc202f 414 const unsigned int DCCinst_forward_fast = 0x6C; //Forward fast speed (step 22)
mglmx 51:badef9fc202f 415 const unsigned int DCCinst_forward_full = 0x6F; //Forward full speed
carlosperales95 25:a42a1ed4d8e9 416 const unsigned int DCCinst_reverse = 0x48; //reverse half speed
carlosperales95 25:a42a1ed4d8e9 417 const unsigned int DCCinst_stop = 0x50; //stop the train
carlosperales95 25:a42a1ed4d8e9 418
carlosperales95 25:a42a1ed4d8e9 419 //100DDDDD for basic headlight functions
carlosperales95 25:a42a1ed4d8e9 420 const unsigned int DCC_func_lighton = 0x90; //F0 turns on headlight function
carlosperales95 25:a42a1ed4d8e9 421 const unsigned int DCC_func_dimlight = 0x91; //F0 + F1 dims headlight
carlosperales95 25:a42a1ed4d8e9 422
carlosperales95 25:a42a1ed4d8e9 423
carlosperales95 25:a42a1ed4d8e9 424 //.....SWITCH COMMAND VARS
carlosperales95 25:a42a1ed4d8e9 425
carlosperales95 25:a42a1ed4d8e9 426 const unsigned int SWBaddress = 0x06; //Address for switch box
carlosperales95 25:a42a1ed4d8e9 427
carlosperales95 25:a42a1ed4d8e9 428 //100DDDDD where DDDDD is the switch command and 100 is constant:
carlosperales95 25:a42a1ed4d8e9 429
carlosperales95 25:a42a1ed4d8e9 430 //00001(F1 active)-00010(F2 active)-00100(F3 active)-01000(F4 active)
carlosperales95 25:a42a1ed4d8e9 431 //Example - 111111 0 00000101 0 10000000 0 10000101 1 - idle
carlosperales95 25:a42a1ed4d8e9 432 const unsigned int SWBidle = 0x80; //IDLE - Flip last activated SW.
carlosperales95 25:a42a1ed4d8e9 433 const unsigned int SWBflip_1 = 0x81; //Flip SW1
carlosperales95 25:a42a1ed4d8e9 434 const unsigned int SWBflip_2 = 0x82; //Flip SW2
carlosperales95 25:a42a1ed4d8e9 435 const unsigned int SWBflip_3 = 0x84; //Flip SW3
carlosperales95 25:a42a1ed4d8e9 436 const unsigned int SWBflip_4 = 0x88; //Flip SW4
carlosperales95 25:a42a1ed4d8e9 437
carlosperales95 40:9acc1341456a 438
mglmx 57:ee5da8a011e0 439 //.....DCC TRAIN COMMAND VARS
mglmx 57:ee5da8a011e0 440
mglmx 57:ee5da8a011e0 441 //typical out of box default engine DCC address is 3 (at least for Bachmann trains)
mglmx 57:ee5da8a011e0 442 //Note: A DCC controller can reprogram the address whenever needed
mglmx 57:ee5da8a011e0 443 const unsigned int DCCaddressDR = 0x01; //Address for train 1 DARK-RED
mglmx 57:ee5da8a011e0 444 const unsigned int DCCaddressLR = 0x03; //Address for train 3 LIGHT-RED
mglmx 57:ee5da8a011e0 445
carlosperales95 45:d589318238bf 446 /**
carlosperales95 45:d589318238bf 447 *Creation of 2 Train objects.
carlosperales95 45:d589318238bf 448 *Using boolean constructor because position initialization will be done after initializing all position vectors.
carlosperales95 45:d589318238bf 449 *DR_train = Dark Red train - LR_train = Light Red Train
carlosperales95 45:d589318238bf 450 **/
mglmx 66:45a90a945983 451 Train DR_train(DCCaddressDR,MEDIUM);
mglmx 57:ee5da8a011e0 452 Train LR_train(DCCaddressLR,MEDIUM);
mglmx 43:346a1f4144cd 453
carlosperales95 47:e992a129ef44 454 //possibility of an array having {dr_train, lr_train}? for reuse and modularity of functions
carlosperales95 47:e992a129ef44 455
carlosperales95 69:609c69e91b4e 456 int speedcheck = 0;
carlosperales95 65:8fd4046c6540 457
mglmx 62:6fc0c683cb02 458
mglmx 18:aa43bb62e60f 459
carlosperales95 38:b9aba3715682 460 //**************** FUNCTIONS FOR DENVER TRAIN ****************//
carlosperales95 38:b9aba3715682 461
carlosperales95 38:b9aba3715682 462
mglmx 22:e4153ca757dd 463 /**
carlosperales95 45:d589318238bf 464 *
carlosperales95 25:a42a1ed4d8e9 465 *Activates the buzzer for 0.5 seconds.
carlosperales95 45:d589318238bf 466 *
mglmx 22:e4153ca757dd 467 **/
mglmx 22:e4153ca757dd 468 void doBuzz(){
carlosperales95 25:a42a1ed4d8e9 469
mglmx 22:e4153ca757dd 470 buzz = 1;
mglmx 22:e4153ca757dd 471 wait(0.5);
mglmx 22:e4153ca757dd 472 buzz = 0;
mglmx 22:e4153ca757dd 473 }
mglmx 18:aa43bb62e60f 474
carlosperales95 45:d589318238bf 475
carlosperales95 45:d589318238bf 476 /**
carlosperales95 45:d589318238bf 477 *
carlosperales95 47:e992a129ef44 478 *Initializes every position's vectors (prev_cw and prev_ccw) with the corresponding sensors.
carlosperales95 47:e992a129ef44 479 *prev_cw - Sensors previous to the current in clockwise sense.
carlosperales95 47:e992a129ef44 480 *prev_ccw - Sensors previous to the current in counter-clockwise sense.
carlosperales95 45:d589318238bf 481 *
carlosperales95 45:d589318238bf 482 **/
mglmx 35:cfcfeccb959e 483 void init_positions(){
mglmx 36:9428c72bdd58 484
mglmx 35:cfcfeccb959e 485 d0.add_prev_cw(D1);
mglmx 35:cfcfeccb959e 486 d0.add_prev_ccw(D13);
mglmx 35:cfcfeccb959e 487
mglmx 35:cfcfeccb959e 488 d1.add_prev_cw(D22);
mglmx 35:cfcfeccb959e 489 d1.add_prev_ccw(D0);
mglmx 35:cfcfeccb959e 490
mglmx 35:cfcfeccb959e 491 d22.add_prev_cw(D2);
mglmx 35:cfcfeccb959e 492 d22.add_prev_ccw(D1);
mglmx 35:cfcfeccb959e 493
mglmx 35:cfcfeccb959e 494 d2.add_prev_cw(D21);
mglmx 35:cfcfeccb959e 495 d2.add_prev_ccw(D22);
mglmx 35:cfcfeccb959e 496
mglmx 35:cfcfeccb959e 497 d21.add_prev_cw(D3);
mglmx 35:cfcfeccb959e 498 d21.add_prev_cw(D4);
mglmx 35:cfcfeccb959e 499 d21.add_prev_ccw(D2);
mglmx 35:cfcfeccb959e 500
mglmx 35:cfcfeccb959e 501 d3.add_prev_cw(D9);
mglmx 35:cfcfeccb959e 502 d3.add_prev_ccw(D21);
mglmx 35:cfcfeccb959e 503
mglmx 35:cfcfeccb959e 504 d4.add_prev_cw(D6);
mglmx 35:cfcfeccb959e 505 d4.add_prev_ccw(D21);
mglmx 35:cfcfeccb959e 506
mglmx 35:cfcfeccb959e 507 d5.add_prev_cw(D6);
mglmx 35:cfcfeccb959e 508 d5.add_prev_ccw(D11);
mglmx 35:cfcfeccb959e 509
mglmx 35:cfcfeccb959e 510 d6.add_prev_cw(D7);
mglmx 35:cfcfeccb959e 511 d6.add_prev_ccw(D4);
mglmx 35:cfcfeccb959e 512 d6.add_prev_ccw(D5);
mglmx 35:cfcfeccb959e 513
mglmx 35:cfcfeccb959e 514 d7.add_prev_cw(D8);
mglmx 35:cfcfeccb959e 515 d7.add_prev_ccw(D6);
mglmx 35:cfcfeccb959e 516
mglmx 36:9428c72bdd58 517 d8.add_prev_cw(D9);
mglmx 36:9428c72bdd58 518 d8.add_prev_cw(D10);
mglmx 36:9428c72bdd58 519 d8.add_prev_ccw(D7);
mglmx 36:9428c72bdd58 520
mglmx 35:cfcfeccb959e 521 d9.add_prev_cw(D3);
mglmx 35:cfcfeccb959e 522 d9.add_prev_ccw(D8);
mglmx 35:cfcfeccb959e 523
mglmx 35:cfcfeccb959e 524 d10.add_prev_cw(D12);
mglmx 35:cfcfeccb959e 525 d10.add_prev_ccw(D8);
mglmx 35:cfcfeccb959e 526
mglmx 35:cfcfeccb959e 527 d11.add_prev_cw(D12);
mglmx 35:cfcfeccb959e 528 d11.add_prev_ccw(D5);
mglmx 35:cfcfeccb959e 529
mglmx 35:cfcfeccb959e 530 d12.add_prev_cw(D13);
mglmx 35:cfcfeccb959e 531 d12.add_prev_ccw(D10);
mglmx 35:cfcfeccb959e 532 d12.add_prev_ccw(D11);
mglmx 35:cfcfeccb959e 533
mglmx 35:cfcfeccb959e 534 d13.add_prev_cw(D0);
mglmx 35:cfcfeccb959e 535 d13.add_prev_ccw(D12);
mglmx 36:9428c72bdd58 536
mglmx 36:9428c72bdd58 537 //Initialize array with positions
mglmx 36:9428c72bdd58 538 positions.push_back(d0);
mglmx 36:9428c72bdd58 539 positions.push_back(d1);
mglmx 36:9428c72bdd58 540 positions.push_back(d2);
mglmx 36:9428c72bdd58 541 positions.push_back(d3);
mglmx 36:9428c72bdd58 542 positions.push_back(d4);
mglmx 36:9428c72bdd58 543 positions.push_back(d5);
mglmx 36:9428c72bdd58 544 positions.push_back(d6);
mglmx 36:9428c72bdd58 545 positions.push_back(d7);
mglmx 36:9428c72bdd58 546 positions.push_back(d8);
mglmx 36:9428c72bdd58 547 positions.push_back(d9);
mglmx 36:9428c72bdd58 548 positions.push_back(d10);
mglmx 36:9428c72bdd58 549 positions.push_back(d11);
mglmx 36:9428c72bdd58 550 positions.push_back(d12);
mglmx 36:9428c72bdd58 551 positions.push_back(d13);
mglmx 36:9428c72bdd58 552 positions.push_back(d21);
mglmx 36:9428c72bdd58 553 positions.push_back(d22);
mglmx 35:cfcfeccb959e 554 }
carlosperales95 24:1d71dd8778c4 555
carlosperales95 45:d589318238bf 556
mglmx 22:e4153ca757dd 557 /**
carlosperales95 24:1d71dd8778c4 558 *
carlosperales95 28:71bd4c83c05f 559 *Here we initialize the mcp that will be used to manage the interrupts.
carlosperales95 25:a42a1ed4d8e9 560 *
carlosperales95 24:1d71dd8778c4 561 **/
mglmx 18:aa43bb62e60f 562 void initialize_mcp(){
mglmx 33:24ce12dec157 563 mcp = new MCP23017(i2c,0x40); //Connect to SCL - p28 and SDA - p27 and MPC I2C address 0x40
mglmx 18:aa43bb62e60f 564
mglmx 33:24ce12dec157 565 mcp->_write(IODIRA, (unsigned char )0xff);
mglmx 33:24ce12dec157 566 mcp->_write(IODIRB, (unsigned char )0xff);
mglmx 33:24ce12dec157 567 mcp->_write(IPOLA, (unsigned char )0x00);
mglmx 33:24ce12dec157 568 mcp->_write(IPOLB, (unsigned char )0x00);
mglmx 33:24ce12dec157 569 mcp->_write(DEFVALA, (unsigned char )0xff);
mglmx 33:24ce12dec157 570 mcp->_write(DEFVALB, (unsigned char )0xff);
mglmx 33:24ce12dec157 571 mcp->_write(INTCONA, (unsigned char )0xff);
mglmx 33:24ce12dec157 572 mcp->_write(INTCONB, (unsigned char )0xff);
mglmx 33:24ce12dec157 573 mcp->_write(IOCONA, (unsigned char )0x2);
mglmx 33:24ce12dec157 574 mcp->_write(IOCONB, (unsigned char )0x2);
mglmx 33:24ce12dec157 575 mcp->_write(GPPUA, (unsigned char )0xff);
mglmx 33:24ce12dec157 576 mcp->_write(GPPUB, (unsigned char )0xff);
mglmx 33:24ce12dec157 577
mglmx 18:aa43bb62e60f 578 }
mglmx 18:aa43bb62e60f 579
carlosperales95 45:d589318238bf 580
carlosperales95 24:1d71dd8778c4 581 /**
carlosperales95 24:1d71dd8778c4 582 *
carlosperales95 37:bb15bea420a3 583 *Returns the number of the sensor where the train was detected.
carlosperales95 37:bb15bea420a3 584 *
carlosperales95 47:e992a129ef44 585 *@number -
carlosperales95 47:e992a129ef44 586 *@interrupt -
carlosperales95 47:e992a129ef44 587 *
carlosperales95 37:bb15bea420a3 588 **/
mglmx 34:c9ab2a987734 589 int get_sensor(unsigned int number,int interrupt){
carlosperales95 45:d589318238bf 590
mglmx 34:c9ab2a987734 591 int sensor = -1;
mglmx 34:c9ab2a987734 592
carlosperales95 37:bb15bea420a3 593 for(int i=0; i<8; i++){
mglmx 34:c9ab2a987734 594
carlosperales95 45:d589318238bf 595 if(~number & 1<<i){
carlosperales95 45:d589318238bf 596
mglmx 34:c9ab2a987734 597 sensor = i;
mglmx 34:c9ab2a987734 598 }
mglmx 34:c9ab2a987734 599 }
mglmx 34:c9ab2a987734 600
mglmx 34:c9ab2a987734 601 if(interrupt == 1){
carlosperales95 45:d589318238bf 602
carlosperales95 37:bb15bea420a3 603 sensor+= 8; // Sensors caught by interreupt1 are identified from 8 to 15.
mglmx 34:c9ab2a987734 604 }
carlosperales95 45:d589318238bf 605
mglmx 34:c9ab2a987734 606 return sensor;
mglmx 34:c9ab2a987734 607 }
mglmx 34:c9ab2a987734 608
carlosperales95 65:8fd4046c6540 609
mglmx 62:6fc0c683cb02 610 /**
mglmx 62:6fc0c683cb02 611 *
mglmx 62:6fc0c683cb02 612 *Method to flip the switches
mglmx 62:6fc0c683cb02 613 *
mglmx 62:6fc0c683cb02 614 *@switchId - (1-4)The ID of the switch we want to flip
mglmx 62:6fc0c683cb02 615 *@times - The number of times we want to send the command
mglmx 62:6fc0c683cb02 616 *@activate - True if the switch is going to be activated. False if it needs to go back to rest position.
mglmx 62:6fc0c683cb02 617 *
mglmx 62:6fc0c683cb02 618 **/
mglmx 62:6fc0c683cb02 619 void flip_switch(int switchId, int times, bool activate=true){
mglmx 62:6fc0c683cb02 620
mglmx 62:6fc0c683cb02 621 unsigned int SWBflip = SWBidle; //IDLE - Flip last activated SW.
mglmx 62:6fc0c683cb02 622
mglmx 62:6fc0c683cb02 623 switch(switchId){
mglmx 62:6fc0c683cb02 624
mglmx 62:6fc0c683cb02 625 case 1:
mglmx 62:6fc0c683cb02 626 SWBflip = SWBflip_1; //FLIP SW1
mglmx 62:6fc0c683cb02 627 break;
mglmx 62:6fc0c683cb02 628
mglmx 62:6fc0c683cb02 629 case 2:
mglmx 62:6fc0c683cb02 630 SWBflip = SWBflip_2; //FLIP SW2
mglmx 62:6fc0c683cb02 631 break;
mglmx 62:6fc0c683cb02 632
mglmx 62:6fc0c683cb02 633 case 3:
mglmx 66:45a90a945983 634 SWBflip = SWBidle; //FLIP SW3
mglmx 62:6fc0c683cb02 635 break;
mglmx 62:6fc0c683cb02 636
mglmx 62:6fc0c683cb02 637 case 4:
mglmx 62:6fc0c683cb02 638 SWBflip = SWBflip_4; //FLIP SW4
mglmx 62:6fc0c683cb02 639 break;
mglmx 62:6fc0c683cb02 640
mglmx 62:6fc0c683cb02 641 default:
mglmx 62:6fc0c683cb02 642 break;
mglmx 62:6fc0c683cb02 643 }
mglmx 62:6fc0c683cb02 644
mglmx 62:6fc0c683cb02 645 //Security measure not to burn the switch.
mglmx 66:45a90a945983 646
mglmx 62:6fc0c683cb02 647
mglmx 66:45a90a945983 648 DCC_send_command(SWBaddress,SWBflip,times); //Activating switchç
mglmx 66:45a90a945983 649
mglmx 66:45a90a945983 650 /*
mglmx 66:45a90a945983 651 if(switchId == 3){
mglmx 66:45a90a945983 652 DCC_send_command(SWBaddress,SWBidle,times); //Activating switch
mglmx 66:45a90a945983 653 }
mglmx 66:45a90a945983 654 */
mglmx 66:45a90a945983 655
mglmx 66:45a90a945983 656
mglmx 66:45a90a945983 657
mglmx 62:6fc0c683cb02 658 if(!activate){
mglmx 66:45a90a945983 659 if(switchId == 3){
mglmx 66:45a90a945983 660 DCC_send_command(SWBaddress,SWBflip_3,times); // Exception for switch 3
mglmx 66:45a90a945983 661 }else{
mglmx 66:45a90a945983 662 DCC_send_command(SWBaddress,SWBidle,times);
mglmx 66:45a90a945983 663 }
mglmx 62:6fc0c683cb02 664 }
mglmx 66:45a90a945983 665
mglmx 62:6fc0c683cb02 666 }
mglmx 62:6fc0c683cb02 667
mglmx 62:6fc0c683cb02 668 /**
mglmx 62:6fc0c683cb02 669 * Action to do when NAC is detected
mglmx 62:6fc0c683cb02 670 * Booster is disabled and the buzz is activated
mglmx 62:6fc0c683cb02 671 *
mglmx 62:6fc0c683cb02 672 */
mglmx 62:6fc0c683cb02 673 void NAC_action(){
mglmx 62:6fc0c683cb02 674 enable = 0;
mglmx 62:6fc0c683cb02 675 doBuzz();
mglmx 62:6fc0c683cb02 676 }
mglmx 62:6fc0c683cb02 677
mglmx 62:6fc0c683cb02 678
carlosperales95 45:d589318238bf 679
carlosperales95 42:b445252a772a 680 /**
mglmx 57:ee5da8a011e0 681 *This method will check if there is a non-avoidable frontal collision(NAFC).
carlosperales95 47:e992a129ef44 682 *A NAFC will happen if:
carlosperales95 47:e992a129ef44 683 *
carlosperales95 47:e992a129ef44 684 *Both trains in area A or B with different direction
carlosperales95 47:e992a129ef44 685 *Trains in (D11 and D5) or (D9 and D3) with same direction
carlosperales95 47:e992a129ef44 686 *
mglmx 43:346a1f4144cd 687 */
mglmx 57:ee5da8a011e0 688 bool check_NAC(){
carlosperales95 45:d589318238bf 689
mglmx 43:346a1f4144cd 690 bool NAC = false;
mglmx 43:346a1f4144cd 691
mglmx 57:ee5da8a011e0 692 if((DR_train.is_in_A() && LR_train.is_in_A()) || (DR_train.is_in_B() && LR_train.is_in_B()) ){ //Check if both are in same area
carlosperales95 45:d589318238bf 693
mglmx 43:346a1f4144cd 694 if(DR_train.goes_cw() ^ LR_train.goes_cw()){ //XOR: They must have different values to be true (Different direction)
mglmx 66:45a90a945983 695 lcd.cls();
mglmx 66:45a90a945983 696 lcd.printf("NAC Both area A or B");
mglmx 43:346a1f4144cd 697 NAC = true;
mglmx 43:346a1f4144cd 698 }
mglmx 67:7ff2425141ce 699 }else if(((DR_train.get_position_number() == D11) && (LR_train.get_position_number() == D5 ) ) || ((LR_train.get_position_number() == D5) && (DR_train.get_position_number() == D11 ))){ //Check if they are in position D11 and D5
carlosperales95 45:d589318238bf 700
mglmx 43:346a1f4144cd 701 if(!(DR_train.goes_cw() ^ LR_train.goes_cw())){ // NOT XOR: They must have same values to be true (Same direction)
mglmx 66:45a90a945983 702 lcd.cls();
mglmx 67:7ff2425141ce 703 lcd.printf("NAC D11 and D5");
mglmx 43:346a1f4144cd 704 NAC = true;
mglmx 43:346a1f4144cd 705 }
mglmx 67:7ff2425141ce 706 }else if(((DR_train.get_position_number() == D9) && (LR_train.get_position_number() == D3)) || ((LR_train.get_position_number() == D9) && (DR_train.get_position_number() == D3)) ){//Check if they are in position D9 and D3
carlosperales95 45:d589318238bf 707
mglmx 43:346a1f4144cd 708 if(!(DR_train.goes_cw() ^ LR_train.goes_cw())){ // NOT XOR: They must have same values to be true (Same direction)
mglmx 66:45a90a945983 709 lcd.cls();
mglmx 67:7ff2425141ce 710 lcd.printf("NAC D9 and D3");
mglmx 43:346a1f4144cd 711 NAC = true;
mglmx 43:346a1f4144cd 712 }
mglmx 43:346a1f4144cd 713 }
carlosperales95 45:d589318238bf 714 return NAC;
mglmx 43:346a1f4144cd 715 }
mglmx 43:346a1f4144cd 716
mglmx 62:6fc0c683cb02 717 /**
mglmx 62:6fc0c683cb02 718 * Switch_n switch that needs to switch
carlosperales95 63:86f8dfa007ac 719 * cont_sensor sensor that when activated the stopped train continues
mglmx 62:6fc0c683cb02 720 * switch_sensor sensor where the switch should be activated
mglmx 66:45a90a945983 721 */
mglmx 62:6fc0c683cb02 722 void AFC_action(int switch_n, int cont_sensor, int switch_sensor, Train *stop_train, Train * cont_train ){
mglmx 62:6fc0c683cb02 723 bool send_pack_switch = false;
mglmx 55:aa3baa01f43d 724
mglmx 62:6fc0c683cb02 725 //flip_switch(switch_n,5);
mglmx 55:aa3baa01f43d 726
mglmx 66:45a90a945983 727 if(switch_n == 3){
mglmx 66:45a90a945983 728 DCC_send_command(SWBaddress,SWBflip_3,15); //Activating switchç
mglmx 66:45a90a945983 729 }
mglmx 66:45a90a945983 730
mglmx 62:6fc0c683cb02 731 while(cont_train->get_position_number() != cont_sensor){
mglmx 62:6fc0c683cb02 732 if(cont_train->get_position_number() == switch_sensor){
mglmx 62:6fc0c683cb02 733 send_pack_switch = true;
mglmx 62:6fc0c683cb02 734 }
mglmx 62:6fc0c683cb02 735 stop_train->set_speed(STOP);
mglmx 62:6fc0c683cb02 736 stop_train->run(); //Stopping train on sensor D4 or D10
mglmx 62:6fc0c683cb02 737 cont_train->run();
mglmx 62:6fc0c683cb02 738
mglmx 62:6fc0c683cb02 739 if(send_pack_switch){
mglmx 62:6fc0c683cb02 740 lcd.cls();
mglmx 62:6fc0c683cb02 741 lcd.printf("Switching SW%d",switch_n);
mglmx 62:6fc0c683cb02 742 flip_switch(switch_n,5);
mglmx 62:6fc0c683cb02 743 }
mglmx 55:aa3baa01f43d 744 }
mglmx 62:6fc0c683cb02 745
mglmx 66:45a90a945983 746 if(switch_n == 3){
mglmx 66:45a90a945983 747 DCC_send_command(SWBaddress,SWBflip_3,15); //Activating switch
mglmx 66:45a90a945983 748 }else{
mglmx 66:45a90a945983 749 flip_switch(5,5); //Send IDLE command
mglmx 66:45a90a945983 750 }
mglmx 66:45a90a945983 751
mglmx 66:45a90a945983 752 stop_train->set_speed(MEDIUM);
mglmx 66:45a90a945983 753
mglmx 62:6fc0c683cb02 754
mglmx 55:aa3baa01f43d 755 }
carlosperales95 45:d589318238bf 756
carlosperales95 68:9859a3ff9298 757 /**
carlosperales95 68:9859a3ff9298 758 * Switch_n switch that needs to switch
carlosperales95 68:9859a3ff9298 759 * cont_sensor sensor that when activated the stopped train continues
carlosperales95 68:9859a3ff9298 760 * switch_sensor sensor where the switch should be activated
carlosperales95 68:9859a3ff9298 761 */
carlosperales95 68:9859a3ff9298 762 void ALC_action(int cont_sensor, Train *stop_train, Train * cont_train ){
carlosperales95 68:9859a3ff9298 763
carlosperales95 68:9859a3ff9298 764 //flip_switch(switch_n,5);
carlosperales95 68:9859a3ff9298 765
carlosperales95 68:9859a3ff9298 766 while(cont_train->get_position_number() != cont_sensor){
carlosperales95 68:9859a3ff9298 767
carlosperales95 68:9859a3ff9298 768 stop_train->set_speed(STOP);
carlosperales95 68:9859a3ff9298 769 stop_train->run(); //Stopping train on sensor D4 or D10
carlosperales95 68:9859a3ff9298 770 cont_train->run();
carlosperales95 68:9859a3ff9298 771 }
carlosperales95 68:9859a3ff9298 772
carlosperales95 68:9859a3ff9298 773 stop_train->set_speed(MEDIUM);
carlosperales95 68:9859a3ff9298 774 }
mglmx 66:45a90a945983 775
carlosperales95 58:b60db1092088 776
mglmx 43:346a1f4144cd 777 /**
carlosperales95 47:e992a129ef44 778 *
mglmx 55:aa3baa01f43d 779 *The function will check if there is an Avoidable Frontal Collision (AFC).
carlosperales95 47:e992a129ef44 780 *AFC will occur if:
carlosperales95 47:e992a129ef44 781 *
carlosperales95 47:e992a129ef44 782 *Train in area A(ccw) and train in D4(cw)
carlosperales95 47:e992a129ef44 783 *Train in area A(cw) and train in D10(ccw)
carlosperales95 47:e992a129ef44 784 *Train in area B(cw) and train in D4(ccw)
mglmx 62:6fc0c683cb02 785 *Train in area B(ccw) and train in D10(ccw)
mglmx 62:6fc0c683cb02 786 *
mglmx 62:6fc0c683cb02 787 *stop_train is the train that is going to stop in sensors D4 or D10 until the other train passes
mglmx 62:6fc0c683cb02 788 *cont_train is the train that won't stop and will do the switch
carlosperales95 47:e992a129ef44 789 *
mglmx 66:45a90a945983 790 **/
mglmx 62:6fc0c683cb02 791 bool check_AFC(Train *stop_train, Train *cont_train){ //TODO - Add same for LR train
mglmx 62:6fc0c683cb02 792 bool detected_AFC = false;
mglmx 62:6fc0c683cb02 793 if( stop_train->get_position_number() == D4){
carlosperales95 45:d589318238bf 794
mglmx 62:6fc0c683cb02 795 if(stop_train->goes_cw()){
carlosperales95 45:d589318238bf 796
mglmx 62:6fc0c683cb02 797 if(cont_train->is_in_A() && !cont_train->goes_cw()){
mglmx 62:6fc0c683cb02 798
mglmx 62:6fc0c683cb02 799 lcd.cls();
mglmx 62:6fc0c683cb02 800 lcd.printf("AFC!!! STOP D4 SW2 CONT D3");
carlosperales95 45:d589318238bf 801
mglmx 62:6fc0c683cb02 802 AFC_action(2,D3,D2,stop_train,cont_train);
mglmx 62:6fc0c683cb02 803 //Activate switch2
mglmx 62:6fc0c683cb02 804 //When cont_train is at D3 stop_train continues
mglmx 62:6fc0c683cb02 805
mglmx 62:6fc0c683cb02 806 detected_AFC = true;
mglmx 43:346a1f4144cd 807 }
mglmx 43:346a1f4144cd 808 }else{ //DR goes ccw
carlosperales95 45:d589318238bf 809
mglmx 62:6fc0c683cb02 810 if(cont_train->is_in_B() && cont_train->goes_cw()){
carlosperales95 45:d589318238bf 811
mglmx 62:6fc0c683cb02 812 lcd.cls();
mglmx 62:6fc0c683cb02 813 lcd.printf("AFC!!! STOP D4 SW3 CONT D5");
mglmx 62:6fc0c683cb02 814
mglmx 62:6fc0c683cb02 815 AFC_action(3,D5,D6,stop_train,cont_train);
mglmx 43:346a1f4144cd 816 //DR_train stops
mglmx 43:346a1f4144cd 817 //Activate switch3
mglmx 43:346a1f4144cd 818 //When LR is at D5 DR continues
mglmx 62:6fc0c683cb02 819 detected_AFC = true;
mglmx 43:346a1f4144cd 820 }
mglmx 43:346a1f4144cd 821 }
mglmx 43:346a1f4144cd 822
mglmx 62:6fc0c683cb02 823 }else if(stop_train->get_position_number() == D10){
carlosperales95 45:d589318238bf 824
mglmx 62:6fc0c683cb02 825 if(stop_train->goes_cw()){
carlosperales95 45:d589318238bf 826
mglmx 62:6fc0c683cb02 827 if(cont_train->is_in_B() && !cont_train->goes_cw()){
mglmx 62:6fc0c683cb02 828 lcd.cls();
mglmx 62:6fc0c683cb02 829 lcd.printf("AFC!!! STOP D10 SW4 CONT D9");
carlosperales95 45:d589318238bf 830
mglmx 62:6fc0c683cb02 831 AFC_action(4,D9,D8,stop_train,cont_train);
mglmx 43:346a1f4144cd 832 //DR train stops
mglmx 43:346a1f4144cd 833 //Activate switch4
mglmx 43:346a1f4144cd 834 //When LR is at D9 DR continues
mglmx 62:6fc0c683cb02 835 detected_AFC = true;
mglmx 43:346a1f4144cd 836 }
mglmx 43:346a1f4144cd 837 }else{
carlosperales95 45:d589318238bf 838
mglmx 62:6fc0c683cb02 839 if(cont_train->is_in_A() && cont_train->goes_cw()){
mglmx 62:6fc0c683cb02 840 lcd.cls();
mglmx 62:6fc0c683cb02 841 lcd.printf("AFC!!! STOP D10 SW1 CONT D11");
mglmx 62:6fc0c683cb02 842 AFC_action(1,D11,D12,stop_train,cont_train);
mglmx 43:346a1f4144cd 843 //DR train stops
mglmx 43:346a1f4144cd 844 //Activate switch1
mglmx 62:6fc0c683cb02 845 //When LR is at D11 DR continues
mglmx 62:6fc0c683cb02 846 detected_AFC = true;
mglmx 43:346a1f4144cd 847 }
mglmx 43:346a1f4144cd 848 }
carlosperales95 68:9859a3ff9298 849 }else if(stop_train->get_position_number() == D9){
carlosperales95 68:9859a3ff9298 850
carlosperales95 68:9859a3ff9298 851 if(stop_train->goes_cw()){
carlosperales95 68:9859a3ff9298 852
carlosperales95 68:9859a3ff9298 853 if(cont_train->is_in_B() && !cont_train->goes_cw()){
carlosperales95 68:9859a3ff9298 854
carlosperales95 68:9859a3ff9298 855 lcd.cls();
carlosperales95 68:9859a3ff9298 856 lcd.printf("AFC!!! STOP D9 CONT D8");
carlosperales95 68:9859a3ff9298 857 AFC_action(5, D10, D4, stop_train, cont_train);
carlosperales95 68:9859a3ff9298 858 //train in 9 stops
carlosperales95 68:9859a3ff9298 859 //when cont_train is at d10 stop train continues
carlosperales95 68:9859a3ff9298 860 detected_AFC = true;
carlosperales95 68:9859a3ff9298 861 }
carlosperales95 68:9859a3ff9298 862 }
carlosperales95 68:9859a3ff9298 863
carlosperales95 68:9859a3ff9298 864 }else if(stop_train->get_position_number() == D11){
carlosperales95 68:9859a3ff9298 865
carlosperales95 68:9859a3ff9298 866 if(!stop_train->goes_cw()){
carlosperales95 68:9859a3ff9298 867
carlosperales95 68:9859a3ff9298 868 if(cont_train->is_in_A() && cont_train->goes_cw()){
carlosperales95 68:9859a3ff9298 869
carlosperales95 68:9859a3ff9298 870 lcd.cls();
carlosperales95 68:9859a3ff9298 871 lcd.printf("AFC!!! STOP D11 CONT D12");
carlosperales95 68:9859a3ff9298 872 AFC_action(5, D10, D4, stop_train, cont_train);
carlosperales95 68:9859a3ff9298 873 //train in 11 stops
carlosperales95 68:9859a3ff9298 874 //when cont_train is at d10 stop train continues
carlosperales95 68:9859a3ff9298 875 detected_AFC = true;
carlosperales95 68:9859a3ff9298 876 }
carlosperales95 68:9859a3ff9298 877 }
carlosperales95 68:9859a3ff9298 878
carlosperales95 68:9859a3ff9298 879 }else if(stop_train->get_position_number() == D3){
carlosperales95 68:9859a3ff9298 880
carlosperales95 68:9859a3ff9298 881 if(stop_train->goes_cw()){
carlosperales95 68:9859a3ff9298 882
carlosperales95 68:9859a3ff9298 883 if(cont_train->is_in_A() && !cont_train->goes_cw()){
carlosperales95 68:9859a3ff9298 884
carlosperales95 68:9859a3ff9298 885 lcd.cls();
carlosperales95 68:9859a3ff9298 886 lcd.printf("AFC!!! STOP D3 CONT D14");
carlosperales95 68:9859a3ff9298 887 AFC_action(5, D4, D10, stop_train, cont_train);
carlosperales95 68:9859a3ff9298 888 //train in 3 stops
carlosperales95 68:9859a3ff9298 889 //when cont_train is at d4 stop train continues
carlosperales95 68:9859a3ff9298 890 detected_AFC = true;
carlosperales95 68:9859a3ff9298 891 }
carlosperales95 68:9859a3ff9298 892 }
carlosperales95 68:9859a3ff9298 893
carlosperales95 68:9859a3ff9298 894 }else if(stop_train->get_position_number() == D5){
carlosperales95 68:9859a3ff9298 895
carlosperales95 68:9859a3ff9298 896 if(!stop_train->goes_cw()){
carlosperales95 68:9859a3ff9298 897
carlosperales95 68:9859a3ff9298 898 if(cont_train->is_in_B() && cont_train->goes_cw()){
carlosperales95 68:9859a3ff9298 899
carlosperales95 68:9859a3ff9298 900 lcd.cls();
carlosperales95 68:9859a3ff9298 901 lcd.printf("AFC!!! STOP D5 CONT D6");
carlosperales95 68:9859a3ff9298 902 AFC_action(5, D4, D10, stop_train, cont_train);
carlosperales95 68:9859a3ff9298 903 //train in 5 stops
carlosperales95 68:9859a3ff9298 904 //when cont_train is at d4 stop train continues
carlosperales95 68:9859a3ff9298 905 detected_AFC = true;
carlosperales95 68:9859a3ff9298 906 }
carlosperales95 68:9859a3ff9298 907 }
mglmx 43:346a1f4144cd 908 }
mglmx 62:6fc0c683cb02 909 return detected_AFC;
mglmx 66:45a90a945983 910 }
mglmx 43:346a1f4144cd 911
carlosperales95 45:d589318238bf 912
carlosperales95 45:d589318238bf 913 /**
carlosperales95 45:d589318238bf 914 *
carlosperales95 63:86f8dfa007ac 915 *
carlosperales95 63:86f8dfa007ac 916 *
carlosperales95 68:9859a3ff9298 917 **/
carlosperales95 63:86f8dfa007ac 918 bool check_ALC(Train *stop_train, Train *cont_train){ //TODO - Add same for LR train
carlosperales95 64:be1015efd974 919
carlosperales95 63:86f8dfa007ac 920 bool detected_ALC = false;
carlosperales95 63:86f8dfa007ac 921 if( stop_train->get_position_number() == D4){
carlosperales95 63:86f8dfa007ac 922
carlosperales95 63:86f8dfa007ac 923 if(stop_train->goes_cw()){
carlosperales95 63:86f8dfa007ac 924
carlosperales95 63:86f8dfa007ac 925 if(cont_train->get_position_number() == D3 && cont_train->goes_cw()){
carlosperales95 63:86f8dfa007ac 926
carlosperales95 63:86f8dfa007ac 927 lcd.cls();
carlosperales95 63:86f8dfa007ac 928 lcd.printf("ALC!!! STOP D4 CONT D3");
carlosperales95 63:86f8dfa007ac 929
carlosperales95 68:9859a3ff9298 930 ALC_action(2, stop_train, cont_train );
carlosperales95 63:86f8dfa007ac 931 //When cont_train is at D22 stop_train continues
carlosperales95 63:86f8dfa007ac 932
carlosperales95 63:86f8dfa007ac 933 detected_ALC = true;
carlosperales95 63:86f8dfa007ac 934 }
carlosperales95 63:86f8dfa007ac 935 }else{ //DR goes ccw
carlosperales95 63:86f8dfa007ac 936
carlosperales95 63:86f8dfa007ac 937 if(cont_train->get_position_number() == D5 && !cont_train->goes_cw()){
carlosperales95 63:86f8dfa007ac 938
carlosperales95 63:86f8dfa007ac 939 lcd.cls();
carlosperales95 68:9859a3ff9298 940 lcd.printf("ALC!!! STOP D4 SW3 CONT D5");
carlosperales95 63:86f8dfa007ac 941
carlosperales95 68:9859a3ff9298 942 ALC_action(7, stop_train, cont_train );
carlosperales95 63:86f8dfa007ac 943 //Train stops
carlosperales95 63:86f8dfa007ac 944 //When CONT is at D6 DR continues
carlosperales95 68:9859a3ff9298 945 detected_ALC = true;
carlosperales95 63:86f8dfa007ac 946 }
carlosperales95 63:86f8dfa007ac 947 }
carlosperales95 63:86f8dfa007ac 948
carlosperales95 63:86f8dfa007ac 949 }else if(stop_train->get_position_number() == D10){
carlosperales95 63:86f8dfa007ac 950
carlosperales95 63:86f8dfa007ac 951 if(stop_train->goes_cw()){
carlosperales95 63:86f8dfa007ac 952
carlosperales95 64:be1015efd974 953 if(cont_train->get_position_number() == D9 && cont_train->goes_cw()){
carlosperales95 63:86f8dfa007ac 954
carlosperales95 63:86f8dfa007ac 955 lcd.cls();
carlosperales95 68:9859a3ff9298 956 lcd.printf("ALC!!! STOP D10 CONT D9");
carlosperales95 63:86f8dfa007ac 957
carlosperales95 68:9859a3ff9298 958 ALC_action(8, stop_train, cont_train );
carlosperales95 63:86f8dfa007ac 959 //D10 train stops
carlosperales95 63:86f8dfa007ac 960 //When CONT is at D8, D10 continues
carlosperales95 68:9859a3ff9298 961 detected_ALC = true;
carlosperales95 63:86f8dfa007ac 962 }
carlosperales95 63:86f8dfa007ac 963 }else{
carlosperales95 63:86f8dfa007ac 964
carlosperales95 64:be1015efd974 965 if(cont_train->get_position_number() == D11 && !cont_train->goes_cw()){
carlosperales95 63:86f8dfa007ac 966
carlosperales95 63:86f8dfa007ac 967 lcd.cls();
carlosperales95 68:9859a3ff9298 968 lcd.printf("ALC!!! STOP D10 CONT D11");
carlosperales95 63:86f8dfa007ac 969
carlosperales95 68:9859a3ff9298 970 ALC_action(12, stop_train, cont_train );
carlosperales95 63:86f8dfa007ac 971 //D10 train stops
carlosperales95 63:86f8dfa007ac 972 //When CONT is at D12, D10 continues
carlosperales95 68:9859a3ff9298 973 detected_ALC = true;
carlosperales95 63:86f8dfa007ac 974 }
carlosperales95 63:86f8dfa007ac 975 }
carlosperales95 63:86f8dfa007ac 976 }
carlosperales95 63:86f8dfa007ac 977
carlosperales95 64:be1015efd974 978 return detected_ALC;
carlosperales95 63:86f8dfa007ac 979 }
carlosperales95 64:be1015efd974 980
carlosperales95 64:be1015efd974 981
carlosperales95 63:86f8dfa007ac 982 /**
carlosperales95 63:86f8dfa007ac 983 *
carlosperales95 47:e992a129ef44 984 *The method check_position will check if any of the trains is in any of the areas.
carlosperales95 47:e992a129ef44 985 *It will go through all the area vectors (A,B) and call the function in_vector to check inside the vectors.
carlosperales95 45:d589318238bf 986 *
carlosperales95 45:d589318238bf 987 **/
mglmx 57:ee5da8a011e0 988 void check_position(){
mglmx 57:ee5da8a011e0 989
mglmx 57:ee5da8a011e0 990 if(check_NAC()){
mglmx 66:45a90a945983 991 //lcd.cls();
mglmx 66:45a90a945983 992 //lcd.printf("NAC!!!");
mglmx 62:6fc0c683cb02 993 NAC_action();
mglmx 62:6fc0c683cb02 994 }
carlosperales95 68:9859a3ff9298 995 check_AFC(&DR_train,&LR_train);
carlosperales95 68:9859a3ff9298 996 check_AFC(&LR_train,&DR_train);
carlosperales95 68:9859a3ff9298 997 check_ALC(&LR_train,&DR_train);
carlosperales95 68:9859a3ff9298 998 check_ALC(&DR_train,&LR_train);
carlosperales95 68:9859a3ff9298 999
mglmx 62:6fc0c683cb02 1000
mglmx 43:346a1f4144cd 1001
mglmx 43:346a1f4144cd 1002 }
mglmx 43:346a1f4144cd 1003
carlosperales95 45:d589318238bf 1004
carlosperales95 45:d589318238bf 1005 /**
carlosperales95 45:d589318238bf 1006 *
carlosperales95 47:e992a129ef44 1007 *Description
carlosperales95 45:d589318238bf 1008 *
carlosperales95 47:e992a129ef44 1009 *@sensor -
carlosperales95 45:d589318238bf 1010 *
carlosperales95 45:d589318238bf 1011 **/
mglmx 35:cfcfeccb959e 1012 void update_train_pos(int sensor){
carlosperales95 70:594fe5c1ad5c 1013
mglmx 35:cfcfeccb959e 1014 bool found_DR = false;
mglmx 41:4fa6aa29d1ed 1015 bool found_LR = false;
mglmx 62:6fc0c683cb02 1016
mglmx 62:6fc0c683cb02 1017 string DR_dir,LR_dir;
mglmx 49:880c0b9c9c64 1018
mglmx 49:880c0b9c9c64 1019 if(DR_train.goes_cw()){
mglmx 49:880c0b9c9c64 1020 DR_dir = "cw";
mglmx 49:880c0b9c9c64 1021 }else{
mglmx 49:880c0b9c9c64 1022 DR_dir = "ccw";
mglmx 49:880c0b9c9c64 1023 }
mglmx 49:880c0b9c9c64 1024
mglmx 49:880c0b9c9c64 1025 if(LR_train.goes_cw()){
mglmx 49:880c0b9c9c64 1026 LR_dir = "cw";
mglmx 49:880c0b9c9c64 1027 }else{
mglmx 49:880c0b9c9c64 1028 LR_dir = "ccw";
mglmx 55:aa3baa01f43d 1029 }
carlosperales95 70:594fe5c1ad5c 1030
mglmx 62:6fc0c683cb02 1031
mglmx 55:aa3baa01f43d 1032 if(sensor == DR_train.get_position_number() || sensor == LR_train.get_position_number()){
mglmx 62:6fc0c683cb02 1033 led2 = 0;
mglmx 62:6fc0c683cb02 1034 }else{
carlosperales95 70:594fe5c1ad5c 1035
mglmx 55:aa3baa01f43d 1036 lcd.cls();
mglmx 55:aa3baa01f43d 1037 lcd.printf("S:D%d DR%d(",sensor,DR_train.get_position_number());
mglmx 55:aa3baa01f43d 1038
mglmx 55:aa3baa01f43d 1039 //TODO: Do a for to print all next sensors.
mglmx 55:aa3baa01f43d 1040 for(int i=0; i<DR_train.get_next_sensors().size(); i++){
mglmx 55:aa3baa01f43d 1041
mglmx 55:aa3baa01f43d 1042 lcd.printf("%d,",DR_train.get_next_sensors()[i]);
mglmx 55:aa3baa01f43d 1043 }
mglmx 55:aa3baa01f43d 1044
mglmx 55:aa3baa01f43d 1045 lcd.printf(")%s LR%d(",DR_dir,LR_train.get_position_number());
mglmx 55:aa3baa01f43d 1046
mglmx 55:aa3baa01f43d 1047 for(int i=0; i<LR_train.get_next_sensors().size(); i++){
mglmx 55:aa3baa01f43d 1048
mglmx 55:aa3baa01f43d 1049 lcd.printf("%d,",LR_train.get_next_sensors()[i]);
mglmx 55:aa3baa01f43d 1050 }
mglmx 55:aa3baa01f43d 1051
mglmx 55:aa3baa01f43d 1052 lcd.printf(")%s",LR_dir);
mglmx 62:6fc0c683cb02 1053
mglmx 62:6fc0c683cb02 1054
mglmx 52:c08495446f87 1055 //Checking next sensors for DR train
mglmx 55:aa3baa01f43d 1056 for(int i=0; i<DR_train.get_next_sensors().size(); i++){
mglmx 55:aa3baa01f43d 1057
mglmx 55:aa3baa01f43d 1058 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 1059
mglmx 55:aa3baa01f43d 1060 found_DR = true;
mglmx 55:aa3baa01f43d 1061
mglmx 55:aa3baa01f43d 1062
mglmx 55:aa3baa01f43d 1063 if(DR_train.goes_cw()){
mglmx 55:aa3baa01f43d 1064 if(DR_train.get_position_number() == D5 || DR_train.get_position_number() == D11){
mglmx 55:aa3baa01f43d 1065
mglmx 55:aa3baa01f43d 1066 DR_train.set_goes_cw(false); //If train goes cw and passes D5 or D11 we change orientation
mglmx 55:aa3baa01f43d 1067 }
mglmx 55:aa3baa01f43d 1068 }else{
mglmx 55:aa3baa01f43d 1069
mglmx 55:aa3baa01f43d 1070 if(DR_train.get_position_number() == D9 || DR_train.get_position_number() == D3){
mglmx 55:aa3baa01f43d 1071
mglmx 55:aa3baa01f43d 1072 DR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
mglmx 55:aa3baa01f43d 1073 }
mglmx 55:aa3baa01f43d 1074 }
mglmx 55:aa3baa01f43d 1075
mglmx 55:aa3baa01f43d 1076 DR_train.set_position(sensor);
mglmx 55:aa3baa01f43d 1077
mglmx 55:aa3baa01f43d 1078 }
mglmx 55:aa3baa01f43d 1079 }
mglmx 55:aa3baa01f43d 1080
mglmx 55:aa3baa01f43d 1081 //Checking next sensors for LR train
mglmx 55:aa3baa01f43d 1082 for(int i=0; i<LR_train.get_next_sensors().size(); i++){
mglmx 55:aa3baa01f43d 1083
mglmx 55:aa3baa01f43d 1084 if(LR_train.get_next_sensors()[i] == sensor){
carlosperales95 45:d589318238bf 1085
mglmx 55:aa3baa01f43d 1086 found_LR = true;
mglmx 55:aa3baa01f43d 1087
mglmx 55:aa3baa01f43d 1088 if(LR_train.goes_cw()){
carlosperales95 45:d589318238bf 1089
mglmx 55:aa3baa01f43d 1090 if(LR_train.get_position_number() == D5 || LR_train.get_position_number() == D11){
mglmx 55:aa3baa01f43d 1091 LR_train.set_goes_cw(false); //If train goes cw and passes D5 or D11 we change orientation
mglmx 55:aa3baa01f43d 1092 }
mglmx 55:aa3baa01f43d 1093 }else{
mglmx 55:aa3baa01f43d 1094
mglmx 55:aa3baa01f43d 1095 if(LR_train.get_position_number() == D9 || LR_train.get_position_number() == D3 ){
mglmx 55:aa3baa01f43d 1096
mglmx 55:aa3baa01f43d 1097 LR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
mglmx 55:aa3baa01f43d 1098 }
mglmx 41:4fa6aa29d1ed 1099 }
mglmx 55:aa3baa01f43d 1100 LR_train.set_position(sensor);
mglmx 55:aa3baa01f43d 1101
mglmx 55:aa3baa01f43d 1102 }
mglmx 55:aa3baa01f43d 1103 }
mglmx 55:aa3baa01f43d 1104 /*
mglmx 55:aa3baa01f43d 1105 if(found_DR){
mglmx 49:880c0b9c9c64 1106
mglmx 55:aa3baa01f43d 1107 //doBuzz();
mglmx 55:aa3baa01f43d 1108 lcd.cls();
mglmx 55:aa3baa01f43d 1109 lcd.printf("DR is at D%d",DR_train.get_position_number());
mglmx 41:4fa6aa29d1ed 1110 }
mglmx 55:aa3baa01f43d 1111
mglmx 55:aa3baa01f43d 1112 if(found_LR){
mglmx 55:aa3baa01f43d 1113
mglmx 55:aa3baa01f43d 1114 lcd.cls();
mglmx 55:aa3baa01f43d 1115 lcd.printf("LR is at D%d",LR_train.get_position_number());
mglmx 55:aa3baa01f43d 1116 }
mglmx 55:aa3baa01f43d 1117
mglmx 55:aa3baa01f43d 1118 if(!found_DR && !found_LR){
mglmx 55:aa3baa01f43d 1119
mglmx 55:aa3baa01f43d 1120 lcd.cls();
mglmx 55:aa3baa01f43d 1121 lcd.printf("No train before :(");
mglmx 55:aa3baa01f43d 1122 }
mglmx 55:aa3baa01f43d 1123 */
mglmx 62:6fc0c683cb02 1124
mglmx 41:4fa6aa29d1ed 1125 }
mglmx 62:6fc0c683cb02 1126
mglmx 35:cfcfeccb959e 1127 }
mglmx 35:cfcfeccb959e 1128
carlosperales95 37:bb15bea420a3 1129
mglmx 34:c9ab2a987734 1130 /**
mglmx 34:c9ab2a987734 1131 *
carlosperales95 25:a42a1ed4d8e9 1132 *Method to catch interrupts 0
carlosperales95 25:a42a1ed4d8e9 1133 *
carlosperales95 24:1d71dd8778c4 1134 **/
mglmx 33:24ce12dec157 1135 void on_int0_change(){
carlosperales95 25:a42a1ed4d8e9 1136
mglmx 33:24ce12dec157 1137 wait_us(2000);
mglmx 33:24ce12dec157 1138 int sensor_data = mcp->_read(INTCAPA);
mglmx 34:c9ab2a987734 1139 int sensor = get_sensor(sensor_data,0);
mglmx 55:aa3baa01f43d 1140 //lcd.cls();
mglmx 55:aa3baa01f43d 1141 //lcd.printf("int0 0x%x \n Sensor: %d",sensor_data,sensor);
mglmx 35:cfcfeccb959e 1142 update_train_pos(sensor);
mglmx 16:2a2da0e67793 1143 }
mglmx 16:2a2da0e67793 1144
mglmx 35:cfcfeccb959e 1145
carlosperales95 24:1d71dd8778c4 1146 /**
carlosperales95 24:1d71dd8778c4 1147 *
carlosperales95 25:a42a1ed4d8e9 1148 *Method to catch interrupts 1
carlosperales95 25:a42a1ed4d8e9 1149 *
carlosperales95 24:1d71dd8778c4 1150 **/
mglmx 33:24ce12dec157 1151 void on_int1_change(){
carlosperales95 25:a42a1ed4d8e9 1152
mglmx 33:24ce12dec157 1153 wait_us(2000);
mglmx 33:24ce12dec157 1154 int sensor_data = mcp->_read(INTCAPB);
mglmx 34:c9ab2a987734 1155 int sensor = get_sensor(sensor_data,1);
mglmx 55:aa3baa01f43d 1156 //lcd.cls();
mglmx 55:aa3baa01f43d 1157 //lcd.printf("int1 0x%x \n Sensor: %d",sensor_data,sensor);
carlosperales95 65:8fd4046c6540 1158
mglmx 35:cfcfeccb959e 1159 update_train_pos(sensor);
mglmx 16:2a2da0e67793 1160 }
mglmx 26:5c966a0a3e8e 1161
carlosperales95 45:d589318238bf 1162 /**
carlosperales95 45:d589318238bf 1163 *
carlosperales95 47:e992a129ef44 1164 *Clear current interrupts
carlosperales95 45:d589318238bf 1165 *
carlosperales95 45:d589318238bf 1166 **/
mglmx 62:6fc0c683cb02 1167 void init() {
mglmx 26:5c966a0a3e8e 1168
mglmx 33:24ce12dec157 1169 mcp->_read(GPIOA);
mglmx 33:24ce12dec157 1170 mcp->_read(GPIOB); // Register callbacks
mglmx 33:24ce12dec157 1171 int0.fall(&on_int0_change);
mglmx 33:24ce12dec157 1172 int1.fall(&on_int1_change); // Enable interrupts on MCP
mglmx 33:24ce12dec157 1173 mcp->_write(GPINTENA, (unsigned char )0xff);
mglmx 33:24ce12dec157 1174 mcp->_write(GPINTENB, (unsigned char )0xff); // Ready to go!
mglmx 62:6fc0c683cb02 1175 }
mglmx 62:6fc0c683cb02 1176
carlosperales95 11:021210c59a95 1177
carlosperales95 24:1d71dd8778c4 1178
mglmx 62:6fc0c683cb02 1179
carlosperales95 11:021210c59a95 1180
mglmx 62:6fc0c683cb02 1181
carlosperales95 24:1d71dd8778c4 1182 /**
carlosperales95 24:1d71dd8778c4 1183 *
carlosperales95 29:559eb2164488 1184 *Checks if any of the switches of the box has been activated.
carlosperales95 29:559eb2164488 1185 *Calls necessary function and displays LCD text.
carlosperales95 25:a42a1ed4d8e9 1186 *
carlosperales95 24:1d71dd8778c4 1187 **/
mglmx 21:e6f1649add39 1188 void checkSwitch(){
carlosperales95 24:1d71dd8778c4 1189
mglmx 22:e4153ca757dd 1190 if(switch1 == 1){
carlosperales95 24:1d71dd8778c4 1191
carlosperales95 69:609c69e91b4e 1192 //lcd.cls();
carlosperales95 69:609c69e91b4e 1193 //lcd.printf("TRAIN NOW WILL STOP AT STATION");
mglmx 22:e4153ca757dd 1194 }else if(switch2 == 1){
carlosperales95 24:1d71dd8778c4 1195
carlosperales95 69:609c69e91b4e 1196 //lcd.cls();
carlosperales95 69:609c69e91b4e 1197 //lcd.printf("SPEEDCHECKMODE");
carlosperales95 69:609c69e91b4e 1198 //change to speedcheckmode
carlosperales95 69:609c69e91b4e 1199 speedcheck = 1;
carlosperales95 69:609c69e91b4e 1200 }else{
carlosperales95 69:609c69e91b4e 1201
carlosperales95 69:609c69e91b4e 1202 //lcd.cls();
carlosperales95 69:609c69e91b4e 1203 //lcd.printf("NORMAL MODE");
carlosperales95 69:609c69e91b4e 1204 speedcheck = 0;
carlosperales95 69:609c69e91b4e 1205 }if(switch3 == 0){
carlosperales95 24:1d71dd8778c4 1206
carlosperales95 69:609c69e91b4e 1207 if(speedcheck>0){
carlosperales95 69:609c69e91b4e 1208
carlosperales95 69:609c69e91b4e 1209 //lcd.cls();
carlosperales95 69:609c69e91b4e 1210 //lcd.printf("SPEED TRAIN DARK RED");
carlosperales95 69:609c69e91b4e 1211 speedcheck = 2;
carlosperales95 69:609c69e91b4e 1212 }
carlosperales95 24:1d71dd8778c4 1213 }else if(switch4 == 0){
carlosperales95 69:609c69e91b4e 1214
carlosperales95 69:609c69e91b4e 1215 if(speedcheck>0){
carlosperales95 69:609c69e91b4e 1216
carlosperales95 69:609c69e91b4e 1217 //lcd.cls();
carlosperales95 69:609c69e91b4e 1218 //lcd.printf("SPEED TRAIN LIGHT RED");
carlosperales95 69:609c69e91b4e 1219 speedcheck = 3;
carlosperales95 69:609c69e91b4e 1220 }
carlosperales95 24:1d71dd8778c4 1221 }
mglmx 22:e4153ca757dd 1222 }
mglmx 22:e4153ca757dd 1223
carlosperales95 45:d589318238bf 1224
carlosperales95 24:1d71dd8778c4 1225
mglmx 55:aa3baa01f43d 1226
mglmx 55:aa3baa01f43d 1227 /**
mglmx 55:aa3baa01f43d 1228 * Returns a sensor number depending on how many times switch3 flips.
mglmx 55:aa3baa01f43d 1229 * When pressing switch4 it confirms the switch
mglmx 55:aa3baa01f43d 1230 * Init_sensor is the value where we start counting.
mglmx 55:aa3baa01f43d 1231 * string train is the name of the train that will be prited with the sensor
mglmx 55:aa3baa01f43d 1232 */
mglmx 55:aa3baa01f43d 1233 int select_sensor(int init_sensor, string train){
mglmx 55:aa3baa01f43d 1234
mglmx 55:aa3baa01f43d 1235 lcd.cls();
mglmx 57:ee5da8a011e0 1236 lcd.printf("%s SENSOR D%d",train,init_sensor);
mglmx 55:aa3baa01f43d 1237
mglmx 55:aa3baa01f43d 1238 int sensor = init_sensor;
mglmx 52:c08495446f87 1239 bool changed = false;
mglmx 52:c08495446f87 1240 bool exit = false;
mglmx 52:c08495446f87 1241
mglmx 52:c08495446f87 1242 while(!exit){
mglmx 52:c08495446f87 1243 if(switch3 == 0){
mglmx 52:c08495446f87 1244 if(changed){
mglmx 52:c08495446f87 1245 sensor++;
mglmx 52:c08495446f87 1246 sensor=sensor%15; //Only sensors from 0 to 15.
mglmx 52:c08495446f87 1247 changed=false;
mglmx 55:aa3baa01f43d 1248 lcd.cls();
mglmx 55:aa3baa01f43d 1249 lcd.printf("%s: D%d",train,sensor);
mglmx 52:c08495446f87 1250 }
mglmx 62:6fc0c683cb02 1251
carlosperales95 58:b60db1092088 1252 }else{
mglmx 52:c08495446f87 1253 changed = true;
mglmx 55:aa3baa01f43d 1254 wait(0.2);
mglmx 52:c08495446f87 1255 }
mglmx 52:c08495446f87 1256
mglmx 52:c08495446f87 1257 if(switch4 == 0){
mglmx 52:c08495446f87 1258 exit = true;
mglmx 55:aa3baa01f43d 1259 wait(0.2);
mglmx 52:c08495446f87 1260 }
mglmx 52:c08495446f87 1261 }
mglmx 62:6fc0c683cb02 1262
mglmx 52:c08495446f87 1263 return sensor;
mglmx 52:c08495446f87 1264 }
carlosperales95 45:d589318238bf 1265
carlosperales95 69:609c69e91b4e 1266
mglmx 55:aa3baa01f43d 1267 /**
mglmx 55:aa3baa01f43d 1268 * Returns a boolean representing the direction. Everytimew switch3 is 0 it changes the direction.
mglmx 55:aa3baa01f43d 1269 * When switch4 is 0 the selection is confirmed.
mglmx 55:aa3baa01f43d 1270 * Init_going_cw is the initial direction.
mglmx 55:aa3baa01f43d 1271 * Train is the string with the name of the train that will be printed next to the direction
mglmx 55:aa3baa01f43d 1272 */
mglmx 55:aa3baa01f43d 1273 bool select_direction(bool init_going_cw,string train){
mglmx 55:aa3baa01f43d 1274
mglmx 57:ee5da8a011e0 1275 string dir_string;
mglmx 57:ee5da8a011e0 1276
mglmx 57:ee5da8a011e0 1277 if(init_going_cw){
mglmx 57:ee5da8a011e0 1278 dir_string = "cw";
mglmx 57:ee5da8a011e0 1279 }else{
mglmx 57:ee5da8a011e0 1280 dir_string = "ccw";
mglmx 57:ee5da8a011e0 1281 }
mglmx 57:ee5da8a011e0 1282
mglmx 55:aa3baa01f43d 1283 lcd.cls();
mglmx 57:ee5da8a011e0 1284 lcd.printf("%s DIRECTION %s ",train,dir_string);
mglmx 57:ee5da8a011e0 1285
mglmx 55:aa3baa01f43d 1286 bool exit = false;
mglmx 55:aa3baa01f43d 1287 bool going_cw = init_going_cw;
mglmx 55:aa3baa01f43d 1288 bool changed = false;
mglmx 55:aa3baa01f43d 1289
mglmx 55:aa3baa01f43d 1290 while(!exit){
mglmx 55:aa3baa01f43d 1291 if(switch3 == 0){
mglmx 55:aa3baa01f43d 1292 if(changed){
mglmx 55:aa3baa01f43d 1293 going_cw = !going_cw;
mglmx 55:aa3baa01f43d 1294 changed = false;
mglmx 55:aa3baa01f43d 1295 string dir;
mglmx 55:aa3baa01f43d 1296 if(going_cw){
mglmx 55:aa3baa01f43d 1297 dir = "cw";
mglmx 55:aa3baa01f43d 1298 }else{
mglmx 55:aa3baa01f43d 1299 dir = "ccw";
mglmx 55:aa3baa01f43d 1300 }
mglmx 55:aa3baa01f43d 1301 lcd.cls();
mglmx 55:aa3baa01f43d 1302 lcd.printf("%s: %s",train,dir);
mglmx 55:aa3baa01f43d 1303 }
mglmx 55:aa3baa01f43d 1304 }else{
mglmx 55:aa3baa01f43d 1305 changed = true;
mglmx 55:aa3baa01f43d 1306 wait(0.2);
mglmx 55:aa3baa01f43d 1307 }
mglmx 55:aa3baa01f43d 1308
mglmx 55:aa3baa01f43d 1309 if(switch4 == 0){
mglmx 55:aa3baa01f43d 1310 exit = true;
mglmx 55:aa3baa01f43d 1311 wait(0.2);
mglmx 55:aa3baa01f43d 1312 }
mglmx 55:aa3baa01f43d 1313 }
mglmx 62:6fc0c683cb02 1314
mglmx 55:aa3baa01f43d 1315 return going_cw;
mglmx 55:aa3baa01f43d 1316 }
mglmx 55:aa3baa01f43d 1317
carlosperales95 69:609c69e91b4e 1318
carlosperales95 69:609c69e91b4e 1319 /**
carlosperales95 69:609c69e91b4e 1320 *
carlosperales95 69:609c69e91b4e 1321 *
carlosperales95 69:609c69e91b4e 1322 *
carlosperales95 69:609c69e91b4e 1323 **/
carlosperales95 69:609c69e91b4e 1324 void adjustSpeed(Train* train){
carlosperales95 69:609c69e91b4e 1325
carlosperales95 69:609c69e91b4e 1326 float f = pot.read();
carlosperales95 69:609c69e91b4e 1327 float vin = f * 3.3;
carlosperales95 69:609c69e91b4e 1328 lcd.cls();
carlosperales95 69:609c69e91b4e 1329 // lcd.printf("vin: %.4f",vin);
carlosperales95 69:609c69e91b4e 1330
carlosperales95 69:609c69e91b4e 1331 if(0<=vin && vin<0.60){
carlosperales95 69:609c69e91b4e 1332
carlosperales95 69:609c69e91b4e 1333 //speed = slow
carlosperales95 69:609c69e91b4e 1334 train->set_speed(SLOW);
carlosperales95 69:609c69e91b4e 1335 }else if(0.60<vin && vin<1.20){
carlosperales95 69:609c69e91b4e 1336
carlosperales95 69:609c69e91b4e 1337 //speed medium
carlosperales95 69:609c69e91b4e 1338 train->set_speed(MEDIUM);
carlosperales95 69:609c69e91b4e 1339 }else if(1.20<vin && vin<2.20){
carlosperales95 69:609c69e91b4e 1340
carlosperales95 69:609c69e91b4e 1341 //speed fast
carlosperales95 69:609c69e91b4e 1342 train->set_speed(FAST);
carlosperales95 69:609c69e91b4e 1343 }else if(2.20<vin && vin<3.20){
carlosperales95 69:609c69e91b4e 1344
carlosperales95 69:609c69e91b4e 1345 //speed full
carlosperales95 69:609c69e91b4e 1346 train->set_speed(FULL);
carlosperales95 69:609c69e91b4e 1347 }
carlosperales95 69:609c69e91b4e 1348 }
carlosperales95 69:609c69e91b4e 1349
carlosperales95 69:609c69e91b4e 1350 void switch_toSpeed(){
carlosperales95 69:609c69e91b4e 1351
carlosperales95 69:609c69e91b4e 1352 switch(speedcheck){
carlosperales95 69:609c69e91b4e 1353
carlosperales95 69:609c69e91b4e 1354 case 2:
carlosperales95 69:609c69e91b4e 1355 adjustSpeed(&DR_train);
carlosperales95 69:609c69e91b4e 1356
carlosperales95 69:609c69e91b4e 1357 case 3:
carlosperales95 69:609c69e91b4e 1358 adjustSpeed(&LR_train);
carlosperales95 69:609c69e91b4e 1359
carlosperales95 69:609c69e91b4e 1360 }
carlosperales95 69:609c69e91b4e 1361 }
carlosperales95 69:609c69e91b4e 1362
carlosperales95 11:021210c59a95 1363 //**************** MAIN PROGRAM FOR DENVER TRAIN ****************//
carlosperales95 11:021210c59a95 1364
carlosperales95 45:d589318238bf 1365
mglmx 1:0ab26889af9b 1366 int main()
mglmx 1:0ab26889af9b 1367 {
carlosperales95 25:a42a1ed4d8e9 1368 //RISE FOR INTERRUPTS?? NOT WORKING ATM
carlosperales95 25:a42a1ed4d8e9 1369 //int0.rise(&interrupt0);
carlosperales95 25:a42a1ed4d8e9 1370 //int1.rise(&interrupt1);
carlosperales95 25:a42a1ed4d8e9 1371
carlosperales95 25:a42a1ed4d8e9 1372 //Read and display potentiometer
carlosperales95 25:a42a1ed4d8e9 1373 //float f = pot.read();
carlosperales95 25:a42a1ed4d8e9 1374 //float vin = f * 3.3;
carlosperales95 25:a42a1ed4d8e9 1375 //lcd.printf("vin: %.4f",vin);
carlosperales95 25:a42a1ed4d8e9 1376
carlosperales95 25:a42a1ed4d8e9 1377 //0xFFFC //1111111111111100
carlosperales95 25:a42a1ed4d8e9 1378
carlosperales95 56:fe999806787d 1379 enable = 0;
carlosperales95 25:a42a1ed4d8e9 1380
carlosperales95 25:a42a1ed4d8e9 1381 //Led routine to start main program
mglmx 2:f580707c44fa 1382 led1 = 1;
mglmx 22:e4153ca757dd 1383 wait(0.2);
mglmx 2:f580707c44fa 1384 led1 = 0;
mglmx 22:e4153ca757dd 1385 wait(0.2);
mglmx 2:f580707c44fa 1386 led1 = 1;
mglmx 16:2a2da0e67793 1387
mglmx 55:aa3baa01f43d 1388 init_positions();
mglmx 55:aa3baa01f43d 1389
carlosperales95 25:a42a1ed4d8e9 1390 initialize_mcp(); //mcp initialization for interrupts before train running
mglmx 33:24ce12dec157 1391 init();
mglmx 57:ee5da8a011e0 1392
mglmx 55:aa3baa01f43d 1393
carlosperales95 68:9859a3ff9298 1394 int DR_init_sensor = select_sensor(D6,"DR");
mglmx 66:45a90a945983 1395 bool DR_init_dir = select_direction(true,"DR");
mglmx 55:aa3baa01f43d 1396
mglmx 55:aa3baa01f43d 1397 wait(0.5);
mglmx 55:aa3baa01f43d 1398
carlosperales95 68:9859a3ff9298 1399 int LR_init_sensor = select_sensor(D3,"LR");
carlosperales95 68:9859a3ff9298 1400 bool LR_init_dir = select_direction(true,"LR");
mglmx 55:aa3baa01f43d 1401
mglmx 55:aa3baa01f43d 1402 DR_train.set_position(DR_init_sensor);
mglmx 55:aa3baa01f43d 1403 DR_train.set_goes_cw(DR_init_dir);
mglmx 55:aa3baa01f43d 1404
mglmx 55:aa3baa01f43d 1405 LR_train.set_position(LR_init_sensor);
mglmx 55:aa3baa01f43d 1406 LR_train.set_goes_cw(LR_init_dir);
mglmx 55:aa3baa01f43d 1407
mglmx 16:2a2da0e67793 1408
mglmx 55:aa3baa01f43d 1409 string DR_print_dir, LR_print_dir;
mglmx 55:aa3baa01f43d 1410
mglmx 55:aa3baa01f43d 1411 if(DR_train.goes_cw()){
mglmx 55:aa3baa01f43d 1412 DR_print_dir = "cw";
mglmx 55:aa3baa01f43d 1413 }else{
mglmx 55:aa3baa01f43d 1414 DR_print_dir = "ccw";
mglmx 55:aa3baa01f43d 1415 }
mglmx 55:aa3baa01f43d 1416
mglmx 55:aa3baa01f43d 1417 if(LR_train.goes_cw()){
mglmx 55:aa3baa01f43d 1418 LR_print_dir = "cw";
mglmx 55:aa3baa01f43d 1419 }else{
mglmx 55:aa3baa01f43d 1420 LR_print_dir = "ccw";
mglmx 55:aa3baa01f43d 1421 }
mglmx 55:aa3baa01f43d 1422
mglmx 55:aa3baa01f43d 1423 lcd.cls();
mglmx 55:aa3baa01f43d 1424 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 1425
mglmx 55:aa3baa01f43d 1426 wait(2);
mglmx 55:aa3baa01f43d 1427
mglmx 43:346a1f4144cd 1428
carlosperales95 25:a42a1ed4d8e9 1429 //Train light routine to start running
mglmx 43:346a1f4144cd 1430 /*
mglmx 32:e5b732fb8e65 1431 DCC_send_command(DCCaddressDR,DCC_func_lighton,200); // turn light on full
mglmx 52:c08495446f87 1432 DCC_send_command(DCCaddressDR,DCC_func_dimlight,400); // dim light
mglmx 52:c08495446f87 1433 DCC_send_command(DCCaddressDR,DCC_func_lighton,200); // light full again
mglmx 43:346a1f4144cd 1434 */
mglmx 57:ee5da8a011e0 1435
mglmx 57:ee5da8a011e0 1436
mglmx 57:ee5da8a011e0 1437
carlosperales95 25:a42a1ed4d8e9 1438 //LED3 Shows start of route + LCD notif
mglmx 22:e4153ca757dd 1439 led3 = 1; // Entering the while
mglmx 22:e4153ca757dd 1440 lcd.cls();
mglmx 22:e4153ca757dd 1441 lcd.printf("Ready to start");
mglmx 55:aa3baa01f43d 1442 wait(1);
carlosperales95 19:ff21ba3a4dc5 1443
mglmx 62:6fc0c683cb02 1444
carlosperales95 56:fe999806787d 1445 enable = 1;
carlosperales95 56:fe999806787d 1446
mglmx 66:45a90a945983 1447 /*
mglmx 66:45a90a945983 1448 flip_switch(5,15);//Send IDLE command at the beginning
mglmx 66:45a90a945983 1449 flip_switch(1,40);
mglmx 66:45a90a945983 1450 wait(0.5);
mglmx 66:45a90a945983 1451 flip_switch(5,15);
mglmx 66:45a90a945983 1452 flip_switch(2,15);
mglmx 66:45a90a945983 1453 wait(0.5);
mglmx 66:45a90a945983 1454 flip_switch(5,15);
mglmx 66:45a90a945983 1455 flip_switch(3,40);
mglmx 66:45a90a945983 1456 wait(0.5);
mglmx 66:45a90a945983 1457 flip_switch(5,15);
mglmx 66:45a90a945983 1458 flip_switch(4,15);
mglmx 66:45a90a945983 1459 wait(0.5);
mglmx 66:45a90a945983 1460 flip_switch(5,15);
mglmx 66:45a90a945983 1461 */
mglmx 66:45a90a945983 1462
mglmx 62:6fc0c683cb02 1463
carlosperales95 14:7bb998edd819 1464 //Demo for stopping at the station
mglmx 1:0ab26889af9b 1465 while(1) {
mglmx 4:50879dfb82d5 1466
carlosperales95 69:609c69e91b4e 1467
mglmx 55:aa3baa01f43d 1468 checkSwitch(); //Checks for switch commands everytime.
carlosperales95 69:609c69e91b4e 1469 switch_toSpeed();
carlosperales95 69:609c69e91b4e 1470 lcd.cls();
mglmx 55:aa3baa01f43d 1471 check_position();
carlosperales95 25:a42a1ed4d8e9 1472
mglmx 33:24ce12dec157 1473 if(1==0){
mglmx 33:24ce12dec157 1474 //if(station == 1){ //If train is on the sensor at the middle of the station it stops and displays LCD text.
carlosperales95 14:7bb998edd819 1475
mglmx 4:50879dfb82d5 1476 lcd.cls();
carlosperales95 29:559eb2164488 1477 lcd.printf("All aboard\n mind the gap");
mglmx 32:e5b732fb8e65 1478 DCC_send_command(DCCaddressDR,DCCinst_stop,400);
carlosperales95 29:559eb2164488 1479 lcd.cls();
mglmx 4:50879dfb82d5 1480
mglmx 22:e4153ca757dd 1481 }else{
mglmx 57:ee5da8a011e0 1482 DR_train.run();
mglmx 57:ee5da8a011e0 1483 LR_train.run();
mglmx 55:aa3baa01f43d 1484
carlosperales95 25:a42a1ed4d8e9 1485 }
mglmx 1:0ab26889af9b 1486 }
mglmx 3:fe7010b693a0 1487 }