main.cpp

00001 #include "mbed.h"
00002 #include "TextLCD.h"
00003 #include "MCP23017.h"
00004 #include "Train.h"
00005 #include "Switch.h"
00006 #include "Track.h"
00007 #include "Detector.h"
00008 #include <cstdlib>
00009 #include <algorithm>
00010 
00011 //Board 1
00012 /*----------------------------------------------------------------------------
00013 Pin definitions
00014 *----------------------------------------------------------------------------*/
00015 DigitalOut Track(p20);      // train track
00016 
00017 DigitalOut myled1(LED1), myled2(LED2), myled3(LED3), myled4(LED4);
00018 DigitalOut externalLed1(p15), externalLed2(p16), externalLed3(p17), externalLed4(p18);
00019 TextLCD lcd(p22, p21, p23, p24, p25, p26); // lcd
00020 
00021 DigitalIn sw1(p29), sw2(p30), sw3(p11), sw4(p12);
00022 
00023 DigitalOut buzz(p10);
00024 
00025 InterruptIn int0(p13), int1(p14);
00026 I2C i2c(p28, p27);
00027 MCP23017 *mcp;
00028 
00029 //DigitalOut en(p6);
00030 
00031 /*----------------------------------------------------------------------------
00032 Addresses
00033 *----------------------------------------------------------------------------*/
00034 const unsigned int DCCaddress_silver = 0x01;
00035 const unsigned int DCCaddress_red = 0x03;
00036 const unsigned int DCCaddress_switch = 0x06;
00037 
00038 /*----------------------------------------------------------------------------
00039 Train movement
00040 *----------------------------------------------------------------------------*/
00041 //move backwards/reverse
00042 
00043 //speed dial forward
00044 const unsigned int slow_speed = 0x76; //step 4
00045 //const unsigned int DCCinst_step6 = 0x68; //step 6 1/4 speed
00046 const unsigned int normal_speed = 0x78; //step 13 1/2 speed
00047 
00048 const unsigned int high_speed = 0x7A; //high speed
00049 
00050 const unsigned int DCCinst_switch1 = 0x81; //Activate switch1
00051 const unsigned int DCCinst_switch2 = 0x82; //Activate switch2
00052 const unsigned int DCCinst_switch3 = 0x84; //Activate switch3
00053 const unsigned int DCCinst_switch4 = 0x88; //Activate switch4
00054 //stop 
00055 const unsigned int stop = 0x40; //forward and stop 01100000
00056 
00057 static const int cRight[] = {6,7,8};
00058 vector<int> C_right (cRight, cRight + sizeof(cRight) / sizeof(cRight[0]) );
00059 
00060 static const int cLeft[] = {0,1,2,12,13,14,15};
00061 vector<int> C_left (cLeft, cLeft + sizeof(cLeft) / sizeof(cLeft[0]) );
00062 
00063 static const int junction39[] = {3,9};
00064 vector<int> junction_39 (junction39, junction39 + sizeof(junction39) / sizeof(junction39[0]) );
00065 
00066 static const int junction511[] = {5,11};
00067 vector<int> junction_511 (junction511, junction511 + sizeof(junction511) / sizeof(junction511[0]) );
00068 
00069 static const int lane4[] = {4};
00070 vector<int> lane_4 (lane4, lane4 + sizeof(lane4) / sizeof(lane4[0]) );
00071 
00072 static const int lane10[] = {10};
00073 vector<int> lane_10 (lane10, lane10 + sizeof(lane10) / sizeof(lane10[0]) );
00074 
00075 int redStartVal = 10;
00076 int silverStartVal = 15;
00077 
00078 /*----------------------------------------------------------------------------
00079 Function definitions
00080 *----------------------------------------------------------------------------*/
00081 bool readSwitch(DigitalIn theSwitch);
00082 bool sameSection();
00083 int convertHextoDec(int pos, int intnr);
00084 void checkDetector(int inter);
00085 void init_mcp();
00086 void init_interrupt();
00087 void init_trains();
00088 void endFunction();
00089 void executeCase(int pos, Train* currTrain);
00090 Train& assignTrain(int pos);
00091 
00092 //Trains
00093 Train redTrain(DCCaddress_red, normal_speed, redStartVal, high_speed, normal_speed);
00094 Train silverTrain(DCCaddress_silver,normal_speed, silverStartVal, normal_speed, slow_speed);
00095 //Switches
00096 Switch switch1(DCCaddress_switch,DCCinst_switch1);
00097 Switch switch2(DCCaddress_switch,DCCinst_switch2);
00098 Switch switch3(DCCaddress_switch,DCCinst_switch3);
00099 Switch switch4(DCCaddress_switch,DCCinst_switch4);
00100 //Detectors
00101 Detector dect(false);
00102 
00103 /*----------------------------------------------------------------------------
00104 Main
00105 *----------------------------------------------------------------------------*/
00106 int main() {
00107     buzz = 0;
00108     lcd.printf("Start journey");
00109     init_mcp();
00110     init_interrupt();
00111     init_trains();
00112     while(1){ 
00113         redTrain.sendCommand();
00114         silverTrain.sendCommand();
00115         
00116         if(!readSwitch(sw3)){ //Change speed
00117             lcd.cls();
00118             lcd.printf("Go forward");
00119             redTrain.changeSpeed(high_speed);
00120             silverTrain.changeSpeed(normal_speed);
00121         }
00122         
00123         if(redTrain.checkStop()){                       //check if Train stopped
00124             lcd.cls();
00125             lcd.printf("%d", redTrain.getPosition());
00126             executeCase(redTrain.getPosition(), &redTrain);
00127         }
00128         else if(silverTrain.checkStop()){
00129             executeCase(silverTrain.getPosition(), &silverTrain);
00130         }
00131         
00132         if (sameSection()){
00133             lcd.cls();
00134             lcd.printf("SAME SEC");
00135             silverTrain.Stop();
00136             redTrain.Stop();
00137         }     
00138         
00139         if(redTrain.checkStop() && silverTrain.checkStop())  {
00140             lcd.cls();
00141             lcd.printf("Stop loop...");
00142             break;    
00143         } 
00144     }
00145     
00146     endFunction();
00147     return 0;
00148 }
00149 
00150 /*----------------------------------------------------------------------------
00151 Functions
00152 *----------------------------------------------------------------------------*/
00153 int convertHextoDec(int pos, int intnr){
00154     int newPos;
00155     switch(pos){
00156         case 0xfe: 
00157             newPos = 0;
00158             break;
00159         case 0xfd: 
00160             newPos = 1;
00161             break; 
00162         case 0xfb: 
00163             newPos = 2;
00164             break; 
00165         case 0xf7: 
00166             newPos = 3;
00167             break; 
00168         case 0xef: 
00169             newPos = 4;
00170             break; 
00171         case 0xdf: 
00172             newPos = 5;
00173             break; 
00174         case 0xbf: 
00175             newPos = 6;
00176             break; 
00177         case 0x7f: 
00178             newPos = 7;
00179             break; 
00180         default:
00181             return -1;
00182         }
00183         
00184         if(intnr == 1)
00185             newPos += 8;
00186     return newPos;
00187 }
00188 
00189 void endFunction(){
00190     
00191     lcd.printf("Shutting down...");
00192     bool in = true;
00193     while(in){ 
00194         redTrain.sendCommand();
00195         silverTrain.sendCommand();
00196         externalLed2 = 1;
00197        // wait(0.2);
00198         //externalLed2 = 0;
00199         buzz = 1;
00200         if(!readSwitch(sw4)){
00201              in = false;   
00202         }
00203             
00204     }
00205     externalLed2 = 0;    
00206     buzz = 0;
00207 }
00208 
00209 Train& assignTrain(int pos){
00210     
00211     //Check which train got the interupt
00212     if(redTrain.checkInterupt(pos) && !redTrain.checkStop()){
00213         redTrain.setPosition(pos);
00214         return redTrain;
00215     }
00216     else if(silverTrain.checkInterupt(pos) && !silverTrain.checkStop()){
00217         silverTrain.setPosition(pos);
00218         return silverTrain;
00219     }else if(redTrain.checkInterupt(pos) && redTrain.checkStop()){
00220         return redTrain;    
00221     }else if(silverTrain.checkInterupt(pos) && silverTrain.checkStop()){
00222         return silverTrain;    
00223     }
00224     else{
00225         lcd.cls();
00226         lcd.printf("NO TRAIN ASSIGNED");
00227         lcd.printf("%d", pos);
00228         
00229         silverTrain.Stop();
00230         redTrain.Stop();
00231         endFunction();
00232     }
00233 }
00234 
00235 void executeCase(int pos, Train* currTrain){
00236     lcd.cls();
00237     lcd.printf("At ");
00238     lcd.printf("%d", pos);
00239     switch(pos){
00240     case 0:{
00241         currTrain->changeSpeed(currTrain->normalSpeed());
00242         break;}
00243     case 1:{
00244         //TO DO: Check which train is going and only slow down if it started from 2
00245         /*if(!currTrain->isClockwise()){
00246             currTrain->changeSpeed(slow_speed);//Slow down
00247             lcd.printf("slow down");
00248         }*/
00249         //else
00250         currTrain->changeSpeed(currTrain->slowlySpeed());
00251         
00252         break;}
00253     case 2:{
00254        /*
00255        Stop everytime at 2     
00256        */
00257        dect.showReservation();
00258        if(!dect.checkReservation(10) && !dect.checkReservation(11)){
00259             currTrain->Stop();
00260             lcd.printf("Stopping at 2");
00261        }
00262        else{ 
00263                                 
00264            if(!dect.checkReservation(4) && !dect.checkReservation(3)){     //Check if 4 or 3 is reserved
00265                int v1 = rand() % 100;                                       //IF NOT randomize which way to go
00266                if (v1 < 50){                                                //0.25 chance to enable the switch2,  reserve 3,9
00267                     switch2.switchOn();
00268                     dect.makeReservation(junction_39);
00269                 } 
00270                 else {                                                      //0.75 chance to disable switch2, reserve 4
00271                    switch2.switchOff(); 
00272                    dect.makeReservation(lane_4);
00273                 }        
00274             }else if(dect.checkReservation(4)){                             //IF 4 is reserved
00275                 switch2.switchOn();                                        //Enable switch2, reserve 3,9
00276                 dect.makeReservation(junction_39);
00277                     
00278             }else{                                                          //IF 3 reserved 
00279                 switch2.switchOff();                                         //Disable switch2, reserve 4
00280                 dect.makeReservation(lane_4);
00281             }
00282             currTrain->changeSpeed(currTrain->normalSpeed());                          //Go forward
00283         }    
00284         break;}
00285     case 3:{   
00286         switch2.switchOff();                        //Disable switch2
00287         dect.clearReservation(C_left);              //free nr 0,1,2,12,13 
00288         if(!dect.checkReservation(5)){ 
00289             currTrain->changeDirection();               //change direction  
00290             currTrain->changeSpeed(currTrain->slowlySpeed());     //go forward 
00291         }   
00292         else{
00293             currTrain->Stop();
00294         }                                                           
00295         break;
00296         }
00297     case 4:{
00298         dect.clearReservation(C_left);   //free nr 0,1,2,12,13
00299         switch3.switchOn();                 //Turn on switch3
00300         dect.showReservation();
00301         if(currTrain->isClockwise()){
00302             lcd.printf("Switch 3 not activated");
00303             redTrain.Stop();
00304             silverTrain.Stop();    
00305         }
00306         else if(dect.checkReservation(6)){   //Check if 6 is reserved
00307             currTrain->Stop();           //IF yes STOP
00308         }
00309         else{
00310             dect.makeReservation(C_right);  //else reserve 6,7,8 and go forward
00311             currTrain->changeSpeed(currTrain->normalSpeed());
00312         }
00313          break;  
00314         }    
00315     case 5:{
00316         dect.makeReservation(junction_511);
00317         switch3.switchOn();                        //Enable switch3
00318         dect.clearReservation(C_right);              //free nr 6,7,8
00319         
00320         if(!dect.checkReservation(3)){  
00321             currTrain->changeDirection();               //change direction  
00322             currTrain->changeSpeed(currTrain->slowlySpeed());     //go forward 
00323         }   
00324         else{
00325             currTrain->Stop();
00326         }                                                                 
00327         break;
00328         }
00329     case 6:{
00330         if(!currTrain->isClockwise())        //IF CC goes towards 7
00331         {
00332             switch3.switchOn();                                             //Enable switch3
00333             //int arr [3] = {4,5,11};
00334             //vector<int> detectors(arr, arr + sizeof(arr) / sizeof(int));
00335             dect.clearReservation(lane_4); 
00336             dect.clearReservation(junction_511);                              //free nr 4,5,11
00337             currTrain->changeSpeed(currTrain->normalSpeed());                          //Move forward
00338         }
00339         else{                           
00340            if(dect.checkReservation(5)){                           //IF 5 is reserved
00341                 currTrain->Stop();
00342                     
00343             }else{                                                          //else 5 reserved
00344                 switch3.switchOff();                                         //Enable switch3, reserve 4
00345                 dect.makeReservation(junction_511);
00346                 currTrain->changeSpeed(currTrain->normalSpeed());
00347             }
00348         }         
00349         break;
00350         }
00351     case 7:{     
00352         currTrain->changeSpeed(currTrain->slowlySpeed());
00353         break;}
00354     case 8:{    
00355         if(currTrain->isClockwise())        //IF C goes towards 7
00356         {
00357             switch4.switchOff();                                             //Disable switch4
00358             dect.clearReservation(junction_39);                                //Free 3,9,10
00359             currTrain->changeSpeed(currTrain->normalSpeed());                          //Move forward
00360         }
00361        else{                                                               //IF CC        if 10 is reserved stop
00362             if(dect.checkReservation(10)){
00363                 currTrain->Stop();
00364             }else{                
00365                 switch4.switchOff();                                         //Disable switch4, reserve 10
00366                 dect.makeReservation(lane_10);
00367                 currTrain->changeSpeed(currTrain->slowlySpeed());                          //Go forward
00368             }
00369         }         
00370         break;
00371         }
00372     case 9:{    
00373         if(!currTrain->isClockwise()){
00374             lcd.printf("Switch 4 is activated");
00375             redTrain.Stop();
00376             silverTrain.Stop();    
00377         }
00378         else if(!dect.checkReservation(8)){
00379             switch4.switchOn();         //Enable switch4
00380             dect.makeReservation(C_right);                                 //Reserve 6,7,8
00381             currTrain->changeSpeed(currTrain->normalSpeed());                    //Go forward
00382 
00383         }
00384         else{
00385             lcd.printf("8 is reserved");         
00386             currTrain->Stop();       
00387         }
00388         break;
00389         }
00390     case 10:{   
00391         dect.makeReservation(lane_10);
00392         dect.clearReservation(C_right); //free nr 6, 7, 8, 9
00393         if(dect.checkReservation(12)){   //Check if 12 is reserved
00394             currTrain->Stop();           //IF yes STOP
00395         }
00396         else{     
00397             switch1.switchOff();          //Turn off switch1                     
00398             dect.makeReservation(C_left);
00399             currTrain->changeSpeed(currTrain->normalSpeed());      //go forward
00400         }     
00401         break;
00402         }
00403         
00404     case 11:{      
00405         dect.makeReservation(junction_511);
00406         dect.showReservation();
00407         if(!dect.checkReservation(12)){
00408             
00409             switch1.switchOn();                        //Enable switch1
00410             dect.makeReservation(C_left);               //Reserve 0,1,2, 12, 13   
00411             currTrain->changeSpeed(currTrain->normalSpeed());         //go forward   
00412         }
00413         else{
00414             currTrain->Stop();   
00415         }                                                             
00416         break;
00417         }
00418     case 12: {      
00419         switch1.switchOff();               //Turn of switch1
00420         //int arr2 [3] = {5,10,11};
00421         //vector<int> detectors2(arr2, arr2 + sizeof(arr2) / sizeof(int));
00422         dect.clearReservation(lane_10); //free nr 5, 10, 11
00423         dect.clearReservation(junction_511);
00424         dect.makeReservation(C_left);
00425         currTrain->changeSpeed(currTrain->normalSpeed());                          //Move forward
00426         
00427         break;
00428         }
00429     case 13: {
00430         currTrain->changeSpeed(currTrain->normalSpeed());
00431         break;
00432         }
00433     case 14:{          
00434         currTrain->changeSpeed(currTrain->slowlySpeed());
00435         break;
00436         }
00437     case 15:{
00438         currTrain->changeSpeed(currTrain->slowlySpeed());
00439         break;
00440         }
00441     default:   {
00442         lcd.printf("Not in a valid case");
00443         currTrain->Stop(); 
00444         break; 
00445         }
00446     }
00447     
00448     mcp->_read(GPIOA);
00449     mcp->_read(GPIOB);
00450 }
00451 
00452 void checkDetector(int inter){
00453     lcd.cls();
00454     int pos;
00455     wait_us(100);
00456     if (inter == 0){
00457         //Read from first one
00458         pos = mcp->_read(INTCAPA);
00459         pos = convertHextoDec(pos, 0);
00460     }
00461     else if (inter == 1){
00462         //Read from second one
00463         pos = mcp->_read(INTCAPB);
00464         pos = convertHextoDec(pos, 1);
00465     }
00466     else{ 
00467         //PROBLEM
00468         lcd.printf("Fail to detect int");
00469         return;
00470     }
00471         
00472     Train* currTrain = &assignTrain(pos);
00473     executeCase(pos, currTrain); 
00474     
00475 }
00476 
00477 bool readSwitch(DigitalIn theSwitch){
00478     int val = theSwitch.read();
00479     if(val == 1)
00480         return true;
00481     else
00482         return false;
00483 }
00484 
00485 void init_mcp() {
00486     // Initialisation of MCP registers,documentation on registers is availableatNiels/Jacco/Natalia
00487     mcp = new MCP23017(i2c, 0x40);
00488     mcp->_write(IODIRA, (unsigned char )0xff);
00489     mcp->_write(IODIRB, (unsigned char )0xff);
00490     mcp->_write(IPOLA, (unsigned char )0x00);
00491     mcp->_write(IPOLB, (unsigned char )0x00);
00492     mcp->_write(DEFVALA, (unsigned char )0xff);
00493     mcp->_write(DEFVALB, (unsigned char )0xff);
00494     mcp->_write(INTCONA, (unsigned char )0xff);
00495     mcp->_write(INTCONB, (unsigned char )0xff);
00496     mcp->_write(IOCONA, (unsigned char )0x2);
00497     mcp->_write(IOCONB, (unsigned char )0x2);
00498     mcp->_write(GPPUA, (unsigned char )0xff);
00499     mcp->_write(GPPUB, (unsigned char )0xff);
00500 }
00501 void passZero(){
00502     checkDetector(0);
00503 }
00504 
00505 void passOne(){
00506     checkDetector(1);
00507 }
00508 
00509 void init_interrupt() {
00510     // Clear current interrupts
00511     //en = 1;
00512     mcp->_read(GPIOA);
00513     mcp->_read(GPIOB);
00514     // Register callbacks
00515     int0.fall(&passZero);
00516     int1.fall(&passOne);
00517     // Enable interrupts on MCP
00518     mcp->_write(GPINTENA, (unsigned char )0xff);
00519     mcp->_write(GPINTENB, (unsigned char )0xff);// Ready to go!
00520 }
00521 
00522 bool sameSection(){
00523     int posRed = redTrain.getPosition();
00524     int posSilver = silverTrain.getPosition();
00525     
00526     if ((std::find(C_left.begin(), C_left.end(), posRed) !=C_left.end()) && (std::find(C_left.begin(), C_left.end(), posSilver) !=C_left.end()))
00527         return true;
00528     else if ((std::find(C_right.begin(), C_right.end(), posRed) !=C_right.end()) && (std::find(C_right.begin(), C_right.end(), posSilver) !=C_right.end()))
00529         return true;
00530     else if ((std::find(junction_511.begin(), junction_511.end(), posRed) !=junction_511.end()) && (std::find(junction_511.begin(), junction_511.end(), posSilver) !=junction_511.end()))
00531         return true;
00532     else if ((std::find(junction_39.begin(), junction_39.end(), posRed) !=junction_39.end()) && (std::find(junction_39.begin(), junction_39.end(), posSilver) !=junction_39.end()))
00533         return true;      
00534          
00535     return false;
00536 }
00537 
00538 void init_trains(){
00539     redTrain.changeSpeed(stop);
00540     silverTrain.changeSpeed(stop);
00541     
00542     vector<int> res;
00543     res.push_back(2);
00544     //res.push_back(4);
00545     dect.makeReservation(C_left); //Run trains counterclockwise and reserve 
00546     vector<int> res2;
00547     res2.push_back(10);
00548     //res2.push_back(13);
00549     dect.makeReservation(res2);
00550     dect.showReservation();
00551 }