Denver / Mbed 2 deprecated denver_train_proj

Important changes to repositories hosted on mbed.com

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

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

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

Dependencies:   mbed TextLCD

main.cpp@60:a1b987ad45fb, 2018-06-25 (annotated)

Committer:
mglmx
Date:
Mon Jun 25 10:33:48 2018 +0000
Revision:
60:a1b987ad45fb
Parent:
57:ee5da8a011e0
Child:
61:ecd2fc4a1a81
AFC half implemented. Needs -SW3 exceptioin and new situations.

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