Denver
/
denver_train_proj
Denver trai project
main.cpp
- Committer:
- mglmx
- Date:
- 2018-06-20
- Revision:
- 49:880c0b9c9c64
- Parent:
- 48:553716e13d45
- Child:
- 50:ee4398ee44be
File content as of revision 49:880c0b9c9c64:
#include "mbed.h" #include "TextLCD.h" #include "MCP23017.h" #include <string> #include <iostream> #include <vector> 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 ///p12 //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 //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 ///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); //MCP MCP23017 *mcp; //------GLOBAL VARS //......SENSOR POSITION VARS //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 /** * *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); }; }; //Creating a vector with all the positions. vector<Position> positions; /** * *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: Position *position; bool going_cw; public: Train(int pos, bool cw){ position = &positions[pos]; going_cw = cw; } 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(); } } 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; } }; //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); /** *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)); //.....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 //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 half 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 /** *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(true); //Position and going_cw Train LR_train(true); //possibility of an array having {dr_train, lr_train}? for reuse and modularity of functions /** *Booleans that will determine if the train should be moving or not. *Booleans will switch to false to stop any of the trains and avoid collisions. *DR_run - Boolean for DR_train / LR_run - Boolean for LR_train **/ bool DR_run = true; bool LR_run = true; //**************** 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; } /** * *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; } /** *This method will checks 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 DR_in_A, bool DR_in_B,bool LR_in_A,bool LR_in_B){ bool NAC = false; if((DR_in_A && LR_in_A) || (DR_in_B && LR_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) NAC = true; } }else if((DR_train.get_position_number() == D11) && (LR_train.get_position_number() == D5 )){ //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) NAC = true; } }else if((DR_train.get_position_number() == D9) && (LR_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) NAC = true; } } return NAC; } /** * *CThe 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) * **/ bool check_AFC(bool DR_in_A, bool DR_in_B,bool LR_in_A,bool LR_in_B){ //TODO - Add same for LR train if( DR_train.get_position_number() == D4){ if(DR_train.goes_cw()){ if(LR_in_A && !LR_train.goes_cw()){ //Activate switch2 //DR_train has to stop //When LR is at D3 DR continues } }else{ //DR goes ccw if(LR_in_B && LR_train.goes_cw()){ //DR_train stops //Activate switch3 //When LR is at D5 DR continues } } }else if(DR_train.get_position_number() == D10){ if(DR_train.goes_cw()){ if(LR_in_B && !LR_train.goes_cw()){ //DR train stops //Activate switch4 //When LR is at D9 DR continues } }else{ if(LR_in_A && LR_train.goes_cw()){ //DR train stops //Activate switch1 //When LR is at D9 DR continues } } } } /** * *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(){ bool DR_in_A, DR_in_B, LR_in_A, LR_in_B; DR_in_A = in_vector(area_A,DR_train.get_position_number()); //Check if DR train is in area A DR_in_B = in_vector(area_B,DR_train.get_position_number()); LR_in_A = in_vector(area_A,LR_train.get_position_number()); LR_in_B = in_vector(area_B,LR_train.get_position_number()); } /** * *Description * *@sensor - * **/ void update_train_pos(int sensor){ bool found_DR = false; bool found_LR = false; 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(",sensor); //TODO: Do a for to print all next sensors. 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(",DR_dir); 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); wait(0.7); //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(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 :("); } } /** * *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); lcd.cls(); lcd.printf("int0 0x%x \n Sensor: %d",sensor_data,sensor); 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); lcd.cls(); lcd.printf("int1 0x%x \n Sensor: %d",sensor_data,sensor); 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! } /** * *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++; } } /** * *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 flipSwitch(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 = SWBflip_3; //FLIP SW3 break; case 4: SWBflip = SWBflip_4; //FLIP SW4 break; default: break; } //Security measure not to burn the switch. if(times <=5){ DCC_send_command(SWBaddress,SWBflip,times); //Activating switch if(!activate){ DCC_send_command(SWBaddress,SWBidle,times); //Sending IDLE to flip back. } } } /** * *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("Switch 1 ON - SW1"); flipSwitch(1,5); }else if(switch2 == 1){ lcd.cls(); lcd.printf("Switch 2 ON - SW2"); flipSwitch(2,5); }else if(switch3 == 0){ lcd.cls(); lcd.printf("Switch 3 ON - SW3"); flipSwitch(3,5); }else if(switch4 == 0){ lcd.cls(); lcd.printf("Switch 4 ON - IDLE"); flipSwitch(0,5); } } /** * * * **/ void send_command(){ if(DR_run){ DCC_send_command(DCCaddressDR,DCCinst_forward,1); // Forward half speed train addres DARK-RED }else{ DCC_send_command(DCCaddressDR,DCCinst_stop,400); } if(LR_run){ DCC_send_command(DCCaddressLR,DCCinst_forward,1); // Forward half speed train addres DARK-RED }else{ DCC_send_command(DCCaddressLR,DCCinst_stop,400); } } //**************** MAIN PROGRAM FOR DENVER TRAIN ****************// int 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 //Led routine to start main program led1 = 1; wait(0.2); led1 = 0; wait(0.2); led1 = 1; initialize_mcp(); //mcp initialization for interrupts before train running init(); init_positions(); DR_train.set_position(D4); LR_train.set_position(D10); //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 */ //LED3 Shows start of route + LCD notif led3 = 1; // Entering the while lcd.cls(); lcd.printf("Ready to start"); //Demo for stopping at the station while(1) { checkSwitch(); //Checks for switch commands everytime. if(1==0){ //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{ send_command(); } } }