Denver / Mbed 2 deprecated denver_train_proj

Denver trai project

Dependencies:   mbed TextLCD

main.cpp@45:d589318238bf, 2018-06-18 (annotated)

Committer:
carlosperales95
Date:
Mon Jun 18 14:16:43 2018 +0000
Revision:
45:d589318238bf
Parent:
44:94870081aece
Child:
46:7a0933676b13
Changes commented code maintenace UPTODATE

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