Denver
/
denver_train_proj
Denver trai project
main.cpp
- Committer:
- carlosperales95
- Date:
- 2018-06-27
- Revision:
- 75:bae748a7905f
- Parent:
- 74:dbbc528f2b18
File content as of revision 75:bae748a7905f:
#include "mbed.h" #include "TextLCD.h" #include "MCP23017.h" #include <string> #include <iostream> #include <vector> #include <fstream> #include <sstream> using namespace std; /******PINS AND DECLARATIONS*******/ //------PINS //SWITCHES p5 - p8 DigitalIn switch1(p5); DigitalIn switch2(p6); DigitalIn switch3(p7); DigitalIn switch4(p8); //RAIL SENSORS - INT0,INT1 //INT0 - p9 InterruptIn int0(p9); //INT1 - p10 InterruptIn int1(p10); ///p11 - EMPTY ///p12 - EMPTY //M0 - p13 DigitalIn d21stat(p13); //Sensor right of the station //M1 - p14 DigitalIn d22stat(p14); //Sensor left of the station //M2 - p15 DigitalIn station(p15); //Sensor in the middle of the station //p16 - EMPTY //ENABLE - p17 DigitalOut enable(p17); //BUZZER - p18 DigitalOut buzz(p18); // buzz=0 doesn't beep, buzz=1 beeps //POTENTIOMETER - p19 AnalogIn pot(p19); //Gives float value pot.read(). Convert analog input to V with f*3.3 //DAT - p20 DigitalOut Track(p20); //Digital output bit used to drive track power via H-bridge //LCD SCREEN - p21, p22, p23, p24, p25, p26 TextLCD lcd(p22,p21,p23,p24,p25,p26); // RS, E, A4, A5, A6, A7 // ldc.cls() to clear and printf(String up to 16char) //p27 - EMPTY //12C - p28 I2C i2c(p28,p27); //LED1 - p29 DigitalOut redled(p29); //LED2 - p30 DigitalOut greenled(p30); //MBED LEDS DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); //MCP MCP23017 *mcp; //------DEFINITIONS //......SENSOR DEFS //Definition of D sensors, will be interpreted as ints for the program's logic #define D0 0 #define D1 1 #define D2 2 #define D3 3 #define D4 4 #define D5 5 #define D6 6 #define D7 7 #define D8 8 #define D9 9 #define D10 10 #define D11 11 #define D12 12 #define D13 13 #define D21 14 #define D22 15 //......SPEED DEFS //Definition of the different train speeds, will be interpreted as ints for the program's logic #define STOP 0 #define SLOW 1 #define MEDIUM 2 #define FAST 3 #define FULL 4 #define R_MEDIUM 5 //--------DCC SEND COMMANDS //01DCSSSS for speed, D is direction (fwd=1 and rev=0), C is speed(SSSSC) LSB const unsigned int DCCinst_forward = 0x68; //forward half speed const unsigned int DCCinst_forward_slow = 0x66; //forward slow speed (step 9) const unsigned int DCCinst_forward_fast = 0x6C; //Forward fast speed (step 22) const unsigned int DCCinst_forward_full = 0x6F; //Forward full speed const unsigned int DCCinst_reverse = 0x48; //reverse half speed const unsigned int DCCinst_stop = 0x50; //stop the train //100DDDDD for basic headlight functions const unsigned int DCC_func_lighton = 0x90; //F0 turns on headlight function const unsigned int DCC_func_dimlight = 0x91; //F0 + F1 dims headlight //.....SWITCH COMMAND VARS const unsigned int SWBaddress = 0x06; //Address for switch box //100DDDDD where DDDDD is the switch command and 100 is constant: //00001(F1 active)-00010(F2 active)-00100(F3 active)-01000(F4 active) //Example - 111111 0 00000101 0 10000000 0 10000101 1 - idle const unsigned int SWBidle = 0x80; //IDLE - Flip last activated SW. const unsigned int SWBflip_1 = 0x81; //Flip SW1 const unsigned int SWBflip_2 = 0x82; //Flip SW2 const unsigned int SWBflip_3 = 0x84; //Flip SW3 const unsigned int SWBflip_4 = 0x88; //Flip SW4 //.....DCC TRAIN COMMAND VARS //typical out of box default engine DCC address is 3 (at least for Bachmann trains) //Note: A DCC controller can reprogram the address whenever needed const unsigned int DCCaddressDR = 0x01; //Address for train 1 DARK-RED const unsigned int DCCaddressLR = 0x03; //Address for train 3 LIGHT-RED /** * *Method to send DCC commands to train and switches. * *@address - (HEX)Address where the commands will be sent *@inst - (HEX)Number of instruction that will be commanded *@repeat_count - Number of times the command will be sent * **/ void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count){ unsigned __int64 command = 0x0000000000000000; // __int64 is the 64-bit integer type unsigned __int64 temp_command = 0x0000000000000000; unsigned __int64 prefix = 0x3FFF; // 14 "1" bits needed at start unsigned int error = 0x00; //error byte //calculate error detection byte with xor error = address ^ inst; //combine packet bits in basic DCC format command = (prefix<<28)|(address<<19)|(inst<<10)|((error)<<1)|0x01; //printf("\n\r %llx \n\r",command); int i=0; //repeat DCC command lots of times while(i < repeat_count) { temp_command = command; //loops through packet bits encoding and sending out digital pulses for a DCC command for (int j=0; j<64; j++) { if((temp_command&0x8000000000000000)==0) { //test packet bit //send data for a "0" bit Track=0; wait_us(100); Track=1; wait_us(100); //printf("0011"); }else{ //send data for a "1"bit Track=0; wait_us(58); Track=1; wait_us(58); //printf("01"); } // next bit in packet temp_command = temp_command<<1; } i++; } } //------CLASSES //......POSITION CLASS /** * *Position class. * *@position - *@previous_cw - *@previous_ccw - * *Position(int) - * *get_pos() - *get_prev_cw() - *get_ccw() - *add_prev_cw() - *add_ccw() - * **/ class Position{ private: int position; vector <int> previous_cw; vector <int> previous_ccw; public: Position(int p){ position = p; } int get_pos(){ return position; } vector<int> get_next_cw(){ return previous_ccw; } vector<int> get_next_ccw(){ return previous_cw; } vector <int> get_prev_cw(){ return previous_cw; } vector <int> get_prev_ccw(){ return previous_ccw; } void add_prev_cw(int pos){ previous_cw.push_back(pos); }; void add_prev_ccw(int pos){ previous_ccw.push_back(pos); }; }; //......INITS USED FOR CLASS TRAIN //Creating a vector with all the positions. vector<Position> positions; /** *Defining areas for train detection and collision logic. *area_A_arr/area_B_arr - Arrays that hold the Dsensors for each area, used to initialize the vectors. *area_A/area_B - Vectors that hold the different sensors of the corresponding areas of the track. **/ int area_A_arr[] = {D21,D2,D22,D1,D0,D13,D12}; int area_B_arr[] = {D6,D7,D8}; const vector<int> area_A(area_A_arr,area_A_arr + sizeof(area_A_arr) / sizeof(int)); const vector<int> area_B(area_B_arr,area_B_arr + sizeof(area_B_arr) / sizeof(int)); //......TRAIN CLASS /** * *Train class. * *@position - *@going_cw - * *Train(int, bool) - *Train(bool) - * *Vector get_next_sensors() - *set_position(int) - *set_goes_cw(bool) - *Position get_position() - *Int get_position_number() - *Bool goes_cw() - * **/ class Train{ private: unsigned int train_address; //stop the train Position *position; bool going_cw; int speed; public: Train(int pos, bool cw){ position = &positions[pos]; going_cw = cw; } /** * Contructor that takes the address of the train and the speed with default value MEDIUM. */ Train(unsigned int address, int s=MEDIUM){ train_address = address; speed = s; } Train(bool cw){ going_cw = cw; } vector<int> get_next_sensors(){ //Checking direction if(going_cw){ return position->get_next_cw(); }else{ return position->get_next_ccw(); } } vector<int> get_previous_sensors(){ //Checking direction if(going_cw){ return position->get_next_ccw(); }else{ return position->get_next_cw(); } } void set_speed(int s){ speed = s; } /** * Sends a DCC command to the train with the speed indicaed by the attribute speed * The number of times the command is sent can be indicated as an optional parameter. Default value is 1. */ void run(int times=1){ const unsigned int DCCinst_forward_slow = 0x66; //forward slow speed (step 9) const unsigned int DCCinst_forward_medium = 0x68; //forward half speed const unsigned int DCCinst_forward_fast = 0x6C; //Forward fast speed (step 22) const unsigned int DCCinst_forward_full = 0x6F; //Forward full speed const unsigned int DCCinst_reverse_medium = 0x48; //reverse half speed const unsigned int DCCinst_stop = 0x50; //stop the train switch(speed){ case STOP: DCC_send_command(train_address, DCCinst_stop,times); break; case SLOW: DCC_send_command(train_address, DCCinst_forward_slow,times); break; case MEDIUM: DCC_send_command(train_address, DCCinst_forward_medium,times); break; case FAST: DCC_send_command(train_address, DCCinst_forward_fast,times); break; case FULL: DCC_send_command(train_address, DCCinst_forward_full,times); break; case R_MEDIUM: DCC_send_command(train_address, DCCinst_reverse_medium,times); break; } } void set_position(int pos){ position = &positions[pos]; //Taking the new position from the positions vector } void set_goes_cw(bool cw){ going_cw = cw; } Position get_position(){ return *position; } int get_position_number(){ return position->get_pos(); } bool goes_cw(){ return going_cw; } /** * *Checks if the element exists within the vector. * *@v - The vector (of ints) the method will go through. *@element - The element the method will look for. * **/ bool in_vector(vector<int>v,int element){ bool exist = false; for(int i=0; i< v.size(); i++){ if(v[i] == element){ exist = true; } } return exist; } bool is_in_A(){ return in_vector(area_A,get_position_number()); } bool is_in_B(){ return in_vector(area_B,get_position_number()); } }; //------GLOBAL VARS AND INITS //......POSITIONS INIT //Creation of all the positions. One for every sensor on the table - Position name(mapping) Position d0(D0); Position d1(D1); Position d2(D2); Position d3(D3); Position d4(D4); Position d5(D5); Position d6(D6); Position d7(D7); Position d8(D8); Position d9(D9); Position d10(D10); Position d11(D11); Position d12(D12); Position d13(D13); Position d21(D21); Position d22(D22); //......TRAINS INIT /** *Creation of 2 Train objects. *Using boolean constructor because position initialization will be done after initializing all position vectors. *DR_train = Dark Red train - LR_train = Light Red Train **/ Train DR_train(DCCaddressDR,MEDIUM); Train LR_train(DCCaddressLR,MEDIUM); //......FLAGS INIT //possibility of an array having {dr_train, lr_train}? for reuse and modularity of functions int speedcheck = 0; bool station_stop = false; //**************** FUNCTIONS FOR DENVER TRAIN ****************// /** * *Activates the buzzer for 0.5 seconds. * **/ void doBuzz(){ buzz = 1; wait(0.5); buzz = 0; } /** * *Initializes every position's vectors (prev_cw and prev_ccw) with the corresponding sensors. *prev_cw - Sensors previous to the current in clockwise sense. *prev_ccw - Sensors previous to the current in counter-clockwise sense. * **/ void init_positions(){ d0.add_prev_cw(D1); d0.add_prev_ccw(D13); d1.add_prev_cw(D22); d1.add_prev_ccw(D0); d22.add_prev_cw(D2); d22.add_prev_ccw(D1); d2.add_prev_cw(D21); d2.add_prev_ccw(D22); d21.add_prev_cw(D3); d21.add_prev_cw(D4); d21.add_prev_ccw(D2); d3.add_prev_cw(D9); d3.add_prev_ccw(D21); d4.add_prev_cw(D6); d4.add_prev_ccw(D21); d5.add_prev_cw(D6); d5.add_prev_ccw(D11); d6.add_prev_cw(D7); d6.add_prev_ccw(D4); d6.add_prev_ccw(D5); d7.add_prev_cw(D8); d7.add_prev_ccw(D6); d8.add_prev_cw(D9); d8.add_prev_cw(D10); d8.add_prev_ccw(D7); d9.add_prev_cw(D3); d9.add_prev_ccw(D8); d10.add_prev_cw(D12); d10.add_prev_ccw(D8); d11.add_prev_cw(D12); d11.add_prev_ccw(D5); d12.add_prev_cw(D13); d12.add_prev_ccw(D10); d12.add_prev_ccw(D11); d13.add_prev_cw(D0); d13.add_prev_ccw(D12); //Initialize array with positions positions.push_back(d0); positions.push_back(d1); positions.push_back(d2); positions.push_back(d3); positions.push_back(d4); positions.push_back(d5); positions.push_back(d6); positions.push_back(d7); positions.push_back(d8); positions.push_back(d9); positions.push_back(d10); positions.push_back(d11); positions.push_back(d12); positions.push_back(d13); positions.push_back(d21); positions.push_back(d22); } /** * *Here we initialize the mcp that will be used to manage the interrupts. * **/ void initialize_mcp(){ mcp = new MCP23017(i2c,0x40); //Connect to SCL - p28 and SDA - p27 and MPC I2C address 0x40 mcp->_write(IODIRA, (unsigned char )0xff); mcp->_write(IODIRB, (unsigned char )0xff); mcp->_write(IPOLA, (unsigned char )0x00); mcp->_write(IPOLB, (unsigned char )0x00); mcp->_write(DEFVALA, (unsigned char )0xff); mcp->_write(DEFVALB, (unsigned char )0xff); mcp->_write(INTCONA, (unsigned char )0xff); mcp->_write(INTCONB, (unsigned char )0xff); mcp->_write(IOCONA, (unsigned char )0x2); mcp->_write(IOCONB, (unsigned char )0x2); mcp->_write(GPPUA, (unsigned char )0xff); mcp->_write(GPPUB, (unsigned char )0xff); } /** * *Returns the number of the sensor where the train was detected. * *@number - *@interrupt - * **/ int get_sensor(unsigned int number,int interrupt){ int sensor = -1; for(int i=0; i<8; i++){ if(~number & 1<<i){ sensor = i; } } if(interrupt == 1){ sensor+= 8; // Sensors caught by interreupt1 are identified from 8 to 15. } return sensor; } /** * *Method to flip the switches * *@switchId - (1-4)The ID of the switch we want to flip *@times - The number of times we want to send the command *@activate - True if the switch is going to be activated. False if it needs to go back to rest position. * **/ void flip_switch(int switchId, int times, bool activate=true){ unsigned int SWBflip = SWBidle; //IDLE - Flip last activated SW. switch(switchId){ case 1: SWBflip = SWBflip_1; //FLIP SW1 break; case 2: SWBflip = SWBflip_2; //FLIP SW2 break; case 3: SWBflip = SWBidle; //FLIP SW3 break; case 4: SWBflip = SWBflip_4; //FLIP SW4 break; default: break; } //Security measure not to burn the switch. DCC_send_command(SWBaddress,SWBflip,times); //Activating switch if(!activate){ if(switchId == 3){ DCC_send_command(SWBaddress,SWBflip_3,times); // Exception for switch 3 }else{ DCC_send_command(SWBaddress,SWBidle,times); } } } /** * Action to do when NAC is detected * Booster is disabled and the buzz is activated * */ void NAC_action(){ enable = 0; doBuzz(); } /** * *This method will check if there is a non-avoidable frontal collision(NAFC). *A NAFC will happen if: * *Both trains in area A or B with different direction *Trains in (D11 and D5) or (D9 and D3) with same direction * **/ bool check_NAC(){ bool NAC = false; 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 if(DR_train.goes_cw() ^ LR_train.goes_cw()){ //XOR: They must have different values to be true (Different direction) lcd.cls(); lcd.printf("NAC Both area A or B"); NAC = true; } }else if(((DR_train.get_position_number() == D11) && (LR_train.get_position_number() == D5 ) ) || ((LR_train.get_position_number() == D5) && (DR_train.get_position_number() == D11 ))){ //Check if they are in position D11 and D5 if(!(DR_train.goes_cw() ^ LR_train.goes_cw())){ // NOT XOR: They must have same values to be true (Same direction) lcd.cls(); lcd.printf("NAC D11 and D5"); NAC = true; } }else if(((DR_train.get_position_number() == D9) && (LR_train.get_position_number() == D3)) || ((LR_train.get_position_number() == D9) && (DR_train.get_position_number() == D3)) ){//Check if they are in position D9 and D3 if(!(DR_train.goes_cw() ^ LR_train.goes_cw())){ // NOT XOR: They must have same values to be true (Same direction) lcd.cls(); lcd.printf("NAC D9 and D3"); NAC = true; } } return NAC; } /** * Switch_n switch that needs to switch * cont_sensor sensor that when activated the stopped train continues * switch_sensor sensor where the switch should be activated */ void AFC_action(int switch_n, int cont_sensor, int switch_sensor, Train *stop_train, Train * cont_train ){ bool send_pack_switch = false; if(switch_n == 3){ DCC_send_command(SWBaddress,SWBflip_3,15); //Activating switchç } while(cont_train->get_position_number() != cont_sensor){ if(cont_train->get_position_number() == switch_sensor){ send_pack_switch = true; } stop_train->set_speed(STOP); stop_train->run(); //Stopping train on sensor D4 or D10 cont_train->run(); if(send_pack_switch){ lcd.cls(); lcd.printf("Switching SW%d",switch_n); flip_switch(switch_n,5); } } if(switch_n == 3){ DCC_send_command(SWBaddress,SWBflip_3,15); //Activating switch }else{ flip_switch(5,5); //Send IDLE command } stop_train->set_speed(MEDIUM); } /** * * Switch_n switch that needs to switch * cont_sensor sensor that when activated the stopped train continues * switch_sensor sensor where the switch should be activated * */ void ALC_action(int cont_sensor, Train *stop_train, Train * cont_train ){ while(cont_train->get_position_number() != cont_sensor){ stop_train->set_speed(STOP); stop_train->run(); //Stopping train on sensor D4 or D10 cont_train->run(); } stop_train->set_speed(MEDIUM); } /** * *The function will check if there is an Avoidable Frontal Collision (AFC). *AFC will occur if: * *Train in area A(ccw) and train in D4(cw) *Train in area A(cw) and train in D10(ccw) *Train in area B(cw) and train in D4(ccw) *Train in area B(ccw) and train in D10(ccw) * *stop_train is the train that is going to stop in sensors D4 or D10 until the other train passes *cont_train is the train that won't stop and will do the switch * **/ bool check_AFC(Train *stop_train, Train *cont_train){ //TODO - Add same for LR train bool detected_AFC = false; if( stop_train->get_position_number() == D4){ if(stop_train->goes_cw()){ if(cont_train->is_in_A() && !cont_train->goes_cw()){ detected_AFC = true; lcd.cls(); lcd.printf("AFC!!! STOP D4 SW2 CONT D3"); AFC_action(2,D3,D2,stop_train,cont_train); //Activate switch2 //When cont_train is at D3 stop_train continues } }else{ //DR goes ccw if(cont_train->is_in_B() && cont_train->goes_cw()){ detected_AFC = true; lcd.cls(); lcd.printf("AFC!!! STOP D4 SW3 CONT D5"); AFC_action(3,D5,D6,stop_train,cont_train); //DR_train stops //Activate switch3 //When LR is at D5 DR continues } } }else if(stop_train->get_position_number() == D10){ if(stop_train->goes_cw()){ if(cont_train->is_in_B() && !cont_train->goes_cw()){ detected_AFC = true; lcd.cls(); lcd.printf("AFC!!! STOP D10 SW4 CONT D9"); AFC_action(4,D9,D8,stop_train,cont_train); //DR train stops //Activate switch4 //When LR is at D9 DR continues } }else{ if(cont_train->is_in_A() && cont_train->goes_cw()){ detected_AFC = true; lcd.cls(); lcd.printf("AFC!!! STOP D10 SW1 CONT D11"); AFC_action(1,D11,D12,stop_train,cont_train); //DR train stops //Activate switch1 //When LR is at D11 DR continues } } }else if(stop_train->get_position_number() == D9){ if(stop_train->goes_cw()){ if(cont_train->is_in_B() && !cont_train->goes_cw()){ detected_AFC = true; lcd.cls(); lcd.printf("AFC!!! STOP D9 CONT D8"); AFC_action(5, D10, D4, stop_train, cont_train); //train in 9 stops //when cont_train is at d10 stop train continues } } }else if(stop_train->get_position_number() == D11){ if(!stop_train->goes_cw()){ if(cont_train->is_in_A() && cont_train->goes_cw()){ detected_AFC = true; lcd.cls(); lcd.printf("AFC!!! STOP D11 CONT D12"); AFC_action(5, D10, D4, stop_train, cont_train); //train in 11 stops //when cont_train is at d10 stop train continues } } }else if(stop_train->get_position_number() == D3){ if(stop_train->goes_cw()){ if(cont_train->is_in_A() && !cont_train->goes_cw()){ detected_AFC = true; lcd.cls(); lcd.printf("AFC!!! STOP D3 CONT D14"); AFC_action(5, D4, D10, stop_train, cont_train); //train in 3 stops //when cont_train is at d4 stop train continues } } }else if(stop_train->get_position_number() == D5){ if(!stop_train->goes_cw()){ if(cont_train->is_in_B() && cont_train->goes_cw()){ detected_AFC = true; lcd.cls(); lcd.printf("AFC!!! STOP D5 CONT D6"); AFC_action(5, D4, D10, stop_train, cont_train); //train in 5 stops //when cont_train is at d4 stop train continues } } } return detected_AFC; } /** * * * **/ bool check_ALC(Train *stop_train, Train *cont_train){ //TODO - Add same for LR train bool detected_ALC = false; if( stop_train->get_position_number() == D4){ if(stop_train->goes_cw()){ if(cont_train->get_position_number() == D3 && cont_train->goes_cw()){ detected_ALC = true; lcd.cls(); lcd.printf("ALC!!! STOP D4 CONT D3"); ALC_action(2, stop_train, cont_train ); //When cont_train is at D22 stop_train continues } }else{ //DR goes ccw if(cont_train->get_position_number() == D5 && !cont_train->goes_cw()){ detected_ALC = true; lcd.cls(); lcd.printf("ALC!!! STOP D4 SW3 CONT D5"); ALC_action(7, stop_train, cont_train ); //Train stops //When CONT is at D6 DR continues } } }else if(stop_train->get_position_number() == D10){ if(stop_train->goes_cw()){ if(cont_train->get_position_number() == D9 && cont_train->goes_cw()){ detected_ALC = true; lcd.cls(); lcd.printf("ALC!!! STOP D10 CONT D9"); ALC_action(8, stop_train, cont_train ); //D10 train stops //When CONT is at D8, D10 continues } }else{ if(cont_train->get_position_number() == D11 && !cont_train->goes_cw()){ detected_ALC = true; lcd.cls(); lcd.printf("ALC!!! STOP D10 CONT D11"); ALC_action(12, stop_train, cont_train ); //D10 train stops //When CONT is at D12, D10 continues } } } return detected_ALC; } /** * * * **/ void stay_at_distance(Train* train_front, Train* train_back){ led1 = 0; led2 = 0; led3 = 0; for(int i=0; i<train_back->get_next_sensors().size(); i++){ for(int j=0; j<train_front->get_previous_sensors().size(); j++){ while(train_back->get_next_sensors()[i] == train_front->get_previous_sensors()[j]){ //lcd.cls(); //lcd.printf("TOO CLOSE! S %d", train_back->get_next_sensors()[i]); led1 = 1; led3 = 1; //stop back train train_back->set_speed(STOP); train_back->run(); train_front->run(); led1 = 0; led2 = 0; led3 = 0; } led1 = 1; led2 = 1; led3 = 1; train_back->set_speed(MEDIUM); train_back->run(); } } led1 = 0; led2 = 0; led3 = 0; } /** * *The method check_position will check if any of the trains is in any of the areas. *It will go through all the area vectors (A,B) and call the function in_vector to check inside the vectors. * **/ void check_position(){ //stay_at_distance(&DR_train,&LR_train); //stay_at_distance(&LR_train,&DR_train); if(check_NAC()){ NAC_action(); } check_AFC(&DR_train,&LR_train); check_AFC(&LR_train,&DR_train); check_ALC(&LR_train,&DR_train); check_ALC(&DR_train,&LR_train); } /** * * * **/ void printPos(int sensor){ string DR_dir,LR_dir; if(DR_train.goes_cw()){ DR_dir = "cw";} else{DR_dir = "ccw";} if(LR_train.goes_cw()){LR_dir = "cw";} else{LR_dir = "ccw";} lcd.cls(); lcd.printf("S:D%d DR%d(",sensor,DR_train.get_position_number()); for(int i=0; i<DR_train.get_next_sensors().size(); i++){ lcd.printf("%d,",DR_train.get_next_sensors()[i]); } lcd.printf(")%s LR%d(",DR_dir,LR_train.get_position_number()); for(int i=0; i<LR_train.get_next_sensors().size(); i++){ lcd.printf("%d,",LR_train.get_next_sensors()[i]); } lcd.printf(")%s",LR_dir); } /** * *Description * *@sensor - * **/ void update_train_pos(int sensor){ //bool found_DR = false; //bool found_LR = false; if(sensor == DR_train.get_position_number() || sensor == LR_train.get_position_number()){ redled = 1; }else{ redled = 0; printPos(sensor); //Checking next sensors for DR train for(int i=0; i<DR_train.get_next_sensors().size(); i++){ if(DR_train.get_next_sensors()[i] == sensor){ //If the sensor is one expected to visit by the train we update the position //found_DR = true; if(DR_train.goes_cw()){ if(DR_train.get_position_number() == D5 || DR_train.get_position_number() == D11){ DR_train.set_goes_cw(false); //If train goes cw and passes D5 or D11 we change orientation } }else{ if(DR_train.get_position_number() == D9 || DR_train.get_position_number() == D3){ DR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation } } DR_train.set_position(sensor); } } //Checking next sensors for LR train for(int i=0; i<LR_train.get_next_sensors().size(); i++){ if(LR_train.get_next_sensors()[i] == sensor){ //found_LR = true; if(LR_train.goes_cw()){ if(LR_train.get_position_number() == D5 || LR_train.get_position_number() == D11){ LR_train.set_goes_cw(false); //If train goes cw and passes D5 or D11 we change orientation } }else{ if(LR_train.get_position_number() == D9 || LR_train.get_position_number() == D3 ){ LR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation } } LR_train.set_position(sensor); if(sensor == D2 && station_stop){ DR_train.set_speed(STOP); LR_train.set_speed(STOP); } } } } } /** * *Method to catch interrupts 0 * **/ void on_int0_change(){ wait_us(2000); int sensor_data = mcp->_read(INTCAPA); int sensor = get_sensor(sensor_data,0); update_train_pos(sensor); } /** * *Method to catch interrupts 1 * **/ void on_int1_change(){ wait_us(2000); int sensor_data = mcp->_read(INTCAPB); int sensor = get_sensor(sensor_data,1); update_train_pos(sensor); } /** * *Clear current interrupts * **/ void init() { mcp->_read(GPIOA); mcp->_read(GPIOB); // Register callbacks int0.fall(&on_int0_change); int1.fall(&on_int1_change); // Enable interrupts on MCP mcp->_write(GPINTENA, (unsigned char )0xff); mcp->_write(GPINTENB, (unsigned char )0xff); // Ready to go! } /** * *Checks if any of the switches of the box has been activated. *Calls necessary function and displays LCD text. * **/ void checkSwitch(){ if(switch1 == 1){ //lcd.cls(); //lcd.printf("TRAIN NOW WILL STOP AT STATION"); station_stop = true; }else{ station_stop = false; LR_train.set_speed(MEDIUM); DR_train.set_speed(MEDIUM); } if(switch2 == 1){ //lcd.cls(); //lcd.printf("SPEEDCHECKMODE"); //change to speedcheckmode speedcheck = 1; }else{ //lcd.cls(); //lcd.printf("NORMAL MODE"); speedcheck = 0; }if(switch3 == 0){ if(speedcheck>0){ //lcd.cls(); //lcd.printf("SPEED TRAIN DARK RED"); speedcheck = 2; } }else if(switch4 == 0){ if(speedcheck>0){ //lcd.cls(); //lcd.printf("SPEED TRAIN LIGHT RED"); speedcheck = 3; } } } /** * * Returns a sensor number depending on how many times switch3 flips. * When pressing switch4 it confirms the switch * Init_sensor is the value where we start counting. * string train is the name of the train that will be prited with the sensor * */ int select_sensor(int init_sensor, string train){ lcd.cls(); lcd.printf("%s SENSOR D%d",train,init_sensor); int sensor = init_sensor; bool changed = false; bool exit = false; while(!exit){ if(switch3 == 0){ if(changed){ sensor++; sensor=sensor%15; //Only sensors from 0 to 15. changed=false; lcd.cls(); lcd.printf("%s: D%d",train,sensor); } }else{ changed = true; wait(0.2); } if(switch4 == 0){ exit = true; wait(0.2); } } return sensor; } /** * Returns a boolean representing the direction. Everytimew switch3 is 0 it changes the direction. * When switch4 is 0 the selection is confirmed. * Init_going_cw is the initial direction. * Train is the string with the name of the train that will be printed next to the direction */ bool select_direction(bool init_going_cw,string train){ string dir_string; if(init_going_cw){dir_string = "cw";} else{dir_string = "ccw";} lcd.cls(); lcd.printf("%s DIRECTION %s ",train,dir_string); bool exit = false; bool going_cw = init_going_cw; bool changed = false; while(!exit){ if(switch3 == 0){ if(changed){ going_cw = !going_cw; changed = false; string dir; if(going_cw){dir = "cw";} else{dir = "ccw";} lcd.cls(); lcd.printf("%s: %s",train,dir); } }else{ changed = true; wait(0.2); } if(switch4 == 0){ exit = true; wait(0.2); } } return going_cw; } /** * * * **/ void adjustSpeed(Train* train){ float f = pot.read(); float vin = f * 3.3; if(0<=vin && vin<0.60){ //speed = slow train->set_speed(SLOW); }else if(0.60<vin && vin<1.20){ //speed medium train->set_speed(MEDIUM); }else if(1.20<vin && vin<2.20){ //speed fast train->set_speed(FAST); }else if(2.20<vin && vin<3.20){ //speed full train->set_speed(FULL); } } /** * * * **/ void switch_toSpeed(){ switch(speedcheck){ case 1: adjustSpeed(&DR_train); adjustSpeed(&LR_train); case 2: adjustSpeed(&DR_train); case 3: adjustSpeed(&LR_train); } } //**************** MAIN PROGRAM FOR DENVER TRAIN ****************// int main() { enable = 0; //Led routine to start main program wait(0.2); led1 = 1; init_positions(); initialize_mcp(); //mcp initialization for interrupts before train running init(); int DR_init_sensor = select_sensor(D4,"DR"); bool DR_init_dir = select_direction(false,"DR"); wait(0.5); int LR_init_sensor = select_sensor(D10,"LR"); bool LR_init_dir = select_direction(false,"LR"); DR_train.set_position(DR_init_sensor); DR_train.set_goes_cw(DR_init_dir); LR_train.set_position(LR_init_sensor); LR_train.set_goes_cw(LR_init_dir); string DR_print_dir, LR_print_dir; if(DR_train.goes_cw()){DR_print_dir = "cw";} else{DR_print_dir = "ccw";} if(LR_train.goes_cw()){LR_print_dir = "cw";} else{LR_print_dir = "ccw";} lcd.cls(); 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); wait(2); //LED2+LED3 Shows start of route + LCD notif led2 = 1; // Entering the while wait(0.4); led3 = 1; // Entering the while wait(0.4); lcd.cls(); lcd.printf("Ready to start"); wait(1); enable = 1; led1 = 0; led2 = 0; led3 = 0; //Demo for stopping at the station while(1) { DR_train.run(); LR_train.run(); check_position(); //checkSwitch(); //Checks for switch commands everytime. //switch_toSpeed(); } } //**********************SAVED CODE CHUNKS****************************// /**Flip switch-idle routine flip_switch(5,15);//Send IDLE command at the beginning flip_switch(1,40); wait(0.5); flip_switch(5,15); flip_switch(2,15); wait(0.5); flip_switch(5,15); flip_switch(3,40); wait(0.5); flip_switch(5,15); flip_switch(4,15); wait(0.5); flip_switch(5,15); **/ /**Code for train to stop at station if(station == 1){ //If train is on the sensor at the middle of the station it stops and displays LCD text. lcd.cls(); lcd.printf("All aboard\n mind the gap"); DCC_send_command(DCCaddressDR,DCCinst_stop,400); lcd.cls(); }else{ DR_train.run(); LR_train.run(); } **/ /**Train light routine to start running DCC_send_command(DCCaddressDR,DCC_func_lighton,200); // turn light on full DCC_send_command(DCCaddressDR,DCC_func_dimlight,400); // dim light DCC_send_command(DCCaddressDR,DCC_func_lighton,200); // light full again **/ /**Print if train is found and updated in update_Train_pos(); if(found_DR){ //doBuzz(); lcd.cls(); lcd.printf("DR is at D%d",DR_train.get_position_number()); } if(found_LR){ lcd.cls(); lcd.printf("LR is at D%d",LR_train.get_position_number()); } if(!found_DR && !found_LR){ lcd.cls(); lcd.printf("No train before :("); } **/ /**Print sensor interrupts in int0 and int1 functions //lcd.cls(); //lcd.printf("int0 0x%x \n Sensor: %d",sensor_data,sensor); **/ /**Print the potentiometer value //lcd.cls(); // lcd.printf("vin: %.4f",vin); **/ /** Chunk 1 of comments from main //RISE FOR INTERRUPTS?? NOT WORKING ATM //int0.rise(&interrupt0); //int1.rise(&interrupt1); //Read and display potentiometer //float f = pot.read(); //float vin = f * 3.3; //lcd.printf("vin: %.4f",vin); //0xFFFC //1111111111111100 **/