main.cpp

00001 #include "mbed.h"
00002 #include <string>
00003 #include <stdarg.h>
00004 #include "TextLCD.h"
00005 #include "MCP23017.h"
00006 #include "math.h"
00007 #include <list>    
00008 
00009 const bool test = false;
00010 int iter;
00011 std::list<int> passedMarkers;
00012 std::list<int>::iterator it;
00013 int speedsetting; 
00014 
00015 //BOX CONNECTIONS
00016 DigitalIn button1 (p8);
00017 DigitalIn button2 (p9);
00018 DigitalIn button3 (p10);
00019 DigitalIn button4 (p30);
00020 AnalogIn potentio (p15); // configures pin15 for analog input. Creates object Ain. It is the potentiometer.
00021 TextLCD lcd(p22, p21, p23, p24, p25, p26); // configures the LCD pins. Creates object lcd
00022 DigitalOut led1(LED1);
00023 DigitalOut led2(LED2);
00024 DigitalOut led3(LED3);
00025 DigitalOut led4(LED4);
00026 DigitalOut boxled1 (p19);
00027 DigitalOut boxled2 (p20);
00028 DigitalOut buzzer (p29);
00029 Serial pc(USBTX, USBRX); // Initialise the serial connection for terminal output
00030 InterruptIn int0(p13);
00031 InterruptIn int1(p14);
00032 I2C i2c(p28, p27);
00033 MCP23017 *mcp;  //hall detectors
00034 DigitalOut data(p16); // configures pin16 for digital output. Creates object train. This signal should go to the booster
00035 DigitalOut enable(p17); //configures pin 17 for digital output. creates object enable, this enables the train track or disables it in case of emergency
00036 
00037 //CONSTANTS
00038 const unsigned int DCCaddress_train1 = 0x01;
00039 const unsigned int DCCaddress_train2 = 0x02;
00040 const unsigned int DCCaddress_decoder = 0x06;
00041 const unsigned int DCCinst_stop = 0x70;         //stop 01 1 1 0000
00042 const unsigned int DCCinst_estop = 0x71;        //01 1 1 0001
00043 const unsigned int DCCswitchneutral = 0b10000000;
00044 const unsigned int DCCswitch1 = 0b10000001;
00045 const unsigned int DCCswitch2 = 0b10000010;
00046 const unsigned int DCCswitch3 = 0b10000100;
00047 const unsigned int DCCswitch4 = 0b10001000;
00048 const double stationWaitTime = 5.;
00049 const double switchsafety = 4.; //contant time limit between uses of same switch
00050 const int switchwait = 1500; //time inbetween different switch commands
00051 
00052 //interrupt flags
00053 bool int0flag;
00054 bool int1flag;
00055 
00056 //global variables for train1
00057 bool dir1;
00058 bool light1;
00059 int speed1;
00060 int prev1;
00061 int curr1;
00062 int next1;
00063 Timer station1;
00064 bool boarding1;
00065 bool command1;
00066 
00067 bool collision;
00068 
00069 //global variables for train2
00070 bool dir2;
00071 bool light2;
00072 int speed2;
00073 int prev2;
00074 int curr2;
00075 int next2;
00076 Timer station2;
00077 bool boarding2;
00078 bool command2;
00079 
00080 //global variables for switches
00081 std::time_t switch1action;
00082 std::time_t switch2action;
00083 std::time_t switch3action;
00084 std::time_t switch4action;
00085 bool switch1;
00086 bool switch2;
00087 bool switch3;
00088 bool switch4;
00089 
00090 //FUNCTION PROTOTYPES
00091 void init_mcp(void);
00092 void on_int0_change(void);
00093 void on_int1_change(void);
00094 std::list<int> circuit0markers(int sensor_data);
00095 std::list<int> circuit1markers(int sensor_data);
00096 
00097 void displayTerminalStatus(void);
00098 void displayLCDstatus(void);
00099 
00100 void init(void);
00101 void initSwitches(void);
00102 void startupWait(void);
00103 void emergencyStop(void);
00104 void handleInt0(void);
00105 void handleInt1(void);
00106 void trainCommand(void);
00107 void updateTrainStatus(int marker);
00108 int predictNextMarker(int prev, int curr);
00109 void iterAction(void);
00110 
00111 void changeSwitch1(bool);
00112 void changeSwitch2(bool);
00113 void changeSwitch3(bool);
00114 void changeSwitch4(bool);
00115 void updateTrainPredictions(void);
00116 
00117 unsigned int trainInstruction(bool direction, bool light, int speed);
00118 void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count);
00119 
00120 void driveTrain(int train);
00121 void reverseTrain(int train);
00122 void stopTrain(int train);
00123 
00124 void atStation2(int train);
00125 void atStation12(int train);
00126 void atSwitch3(int train);
00127 void atSwitch4(int train);
00128 void headCollision(void);
00129 void sideCollision(void);
00130 void xCollision(void);
00131 bool isXCollision(void);
00132 void tailCollision(int backTrain);
00133 void atStation(int train);
00134 //void loopDeltaCrossing(int train);
00135 
00136 int main() {
00137     buzzer = 0;
00138     wait_ms(1000); //safety wait when starting (power surge)
00139     enable = false; //track off while doing startup
00140     while(1){
00141         init(); // initialize all the things to starting values
00142         startupWait(); //wait for user to press button 3 to begin
00143         enable = true;
00144         initSwitches(); //put switches in starting position
00145         wait_ms(2000); //power surge safety
00146         if(int0flag || int1flag){
00147             pc.printf("WARNING: Initialization power surge! Sensor ignored\n\r"); 
00148             if(int0flag){
00149                 int trash = mcp->_read(INTCAPA);
00150                 int0flag = false;
00151             }
00152             if(int1flag){
00153                 int trash = mcp->_read(INTCAPB);
00154                 int1flag = false;
00155             }  
00156         }
00157         
00158         if(!test){
00159             iter = 0; //iter count   
00160             while(enable){
00161                 if(!button4){ 
00162                     emergencyStop();
00163                     break;
00164                 }          
00165                 
00166                 if(int0flag){
00167                     handleInt0(); //store passed circuit 0 markers in the list passed markers
00168                 }
00169             
00170                 if(int1flag){
00171                     handleInt1(); //same for circuit 1
00172                 }
00173                 
00174                 if(!passedMarkers.empty()){ 
00175                     for (it=passedMarkers.begin(); it!=passedMarkers.end(); ++it){
00176                         if(*it != next1 && *it != next2 && *it != curr1 && *it != curr2){  //TODO check if safe
00177                             pc.printf("ERROR: Unexpected marker %d. Expected train1: %d or train2: %d. Sensor failure or unknown object on track. Aborting.\n\r", *it, next1, next2);
00178                             enable = false;
00179                             break;   
00180                         }        
00181                         updateTrainStatus(*it);
00182                     }
00183                     passedMarkers.clear();  
00184                 }
00185                 
00186                 trainCommand(); //decides for a command to send to the track
00187                 
00188                 iterAction(); //every X iterations display some stuff
00189                 iter++;
00190             }
00191         } else {
00192             //test scripts   
00193         }
00194     }
00195 }
00196 
00197 //MAIN LOOP ROUTINES
00198 void init(){  
00199     command1 = false;
00200     command2 = false;
00201     collision = false;
00202       
00203     //initialize clock time
00204     set_time(1);
00205     switch1action = time(NULL);
00206     switch2action = time(NULL);
00207     switch3action = time(NULL);
00208     switch4action = time(NULL);
00209     
00210     led1 = 0;
00211     led2 = 0;
00212     led3 = 0;
00213     buzzer = 0;
00214     boxled1 = 0;
00215     boxled2 = 0;
00216     
00217     speedsetting = 7;
00218     
00219     //pc.baud(921600); //faster printing
00220     pc.baud(19200);
00221     
00222     //initialize mcp   //(initialize interrupt flags)
00223     init_mcp();
00224   
00225     //initialize train globals
00226     dir1 = true;
00227     light1 = true;
00228     speed1 = speedsetting;
00229     boarding1 = false;
00230     //train 1 starting position
00231     prev1 = 0;
00232     curr1 = 1;
00233     next1 = predictNextMarker(prev1, curr1); //2
00234 
00235     //global variables for train2
00236     dir2 = true;
00237     light2 = true;
00238     speed2 = speedsetting;
00239     boarding2 = false;
00240     //train 2 starting position
00241     prev2 = 4;
00242     curr2 = 6;
00243     next2 = predictNextMarker(prev2, curr2); //7
00244 
00245     //initialize presumed switch positions
00246     switch1 = false;
00247     switch2 = false;
00248     switch3 = false;
00249     switch4 = false;
00250 }
00251 
00252 void startupWait(){
00253     lcd.cls();
00254     lcd.printf("Press START when ready");
00255     pc.printf("Press START when ready. Starting position: train 1 between marker 1 and 2, train 2 between marker 6 and 7.\n\r");  
00256     while(button3){
00257         //do nothing   
00258     }
00259     pc.printf("Starting...\n\r");  
00260 }
00261 
00262 void initSwitches(){
00263     wait_ms(switchwait);
00264     changeSwitch1(switch1);
00265     wait_ms(switchwait);
00266     changeSwitch2(switch2);
00267     wait_ms(switchwait);
00268     changeSwitch3(switch3);
00269     wait_ms(switchwait);
00270     changeSwitch4(switch4);
00271     wait_ms(500);
00272 }
00273 
00274 void emergencyStop(){
00275     enable = false;
00276     pc.printf("Emergency stop! Press button 3 when ready to restart\n\r");
00277     
00278     lcd.cls();
00279     lcd.printf("Press START when ready to restart");
00280     while(button3){
00281         //do nothing   
00282     }
00283 }
00284 
00285 void handleInt0(){
00286     wait_us(2000);
00287     int sensor_data = mcp->_read(INTCAPA);
00288     int0flag = false;
00289     std::list<int> temp = circuit0markers(sensor_data);
00290     passedMarkers.splice(passedMarkers.end(), temp);
00291 }
00292 
00293 void handleInt1(){
00294     wait_us(2000);
00295     int sensor_data = mcp->_read(INTCAPB);
00296     int1flag = false;  
00297     std::list<int> temp = circuit1markers(sensor_data);
00298     passedMarkers.splice(passedMarkers.end(), temp); 
00299 }
00300 
00301 void updateTrainStatus(int marker){
00302     if(marker == 21){
00303         return; //marker 21 is unused    
00304     }
00305     
00306     if(marker == next1){
00307         prev1 = curr1;
00308         curr1 = next1;
00309         next1 = predictNextMarker(prev1, curr1);
00310         command1 = false;
00311         collision = false;
00312         pc.printf("STATUS: Train 1 reached marker %d from marker %d. Predicted marker %d\n\r", curr1, prev1, next1);
00313     } else { //marker == next2
00314         prev2 = curr2;
00315         curr2 = next2;
00316         next2 = predictNextMarker(prev2, curr2);
00317         command2 = false;
00318         collision = false;
00319         pc.printf("STATUS: Train 2 reached marker %d from marker %d. Predicted marker %d\n\r", curr2, prev2, next2);
00320     }   
00321 }
00322 
00323 int predictNextMarker(int prev, int curr){
00324     switch(prev){
00325         case 0: switch(curr){
00326                     case 1: return 2;
00327                     case 13: return 12;
00328                 }
00329         case 1: switch(curr){
00330                     case 2: return switch2?3:4;
00331                     case 0: return 13;
00332                 }
00333         case 2: switch(curr){
00334                     case 3: return 9;
00335                     case 4: return 6;
00336                     case 1: return 0;
00337                 }
00338         case 3: switch(curr){
00339                     case 9: return 8;
00340                     case 2: return 1;
00341                 }
00342         case 4: switch(curr){
00343                     case 6: return 7;
00344                     case 2: return 1;
00345                 }
00346         case 5: switch(curr){
00347                     case 6: return 7;
00348                     case 11: return 12;
00349                 }
00350         case 6: switch(curr){
00351                     case 4: return 2;
00352                     case 5: return 11;
00353                     case 7: return 8;
00354                 }
00355         case 7: switch(curr){
00356                     case 6: return switch3?5:4;
00357                     case 8: return switch4?9:10;
00358                 }
00359         case 8: switch(curr){
00360                     case 7: return 6;
00361                     case 9: return 3;
00362                     case 10: return 12;
00363                 }
00364         case 9: switch(curr){
00365                     case 3: return 2;
00366                     case 8: return 7;
00367                 }
00368         case 10: switch(curr){
00369                     case 8: return 7;
00370                     case 12: return 13;
00371                 }
00372         case 11: switch(curr){
00373                     case 5: return 6;
00374                     case 12: return 13;
00375                 }
00376         case 12: switch(curr){
00377                     case 10: return 8;
00378                     case 11: return 5;
00379                     case 13: return 0;
00380                 }
00381         case 13: switch(curr){
00382                     case 0: return 1;
00383                     case 12: return switch1?11:10;
00384                 }       
00385     }
00386     return 666; //TODO default
00387 }  
00388 
00389 void iterAction(){
00390     displayLCDstatus();
00391     if(iter % 100 == 0){
00392         displayTerminalStatus();
00393     }
00394 }
00395 
00396 //TRAIN COMMAND
00397 void trainCommand(){
00398     float Vin = potentio.read() * 3.3;
00399     speedsetting = floor(Vin/0.22);  //0.22 = 3.3/15  
00400     
00401     if(boarding1 || boarding2){
00402         if(station1.read() > stationWaitTime){ //timer works? TODO
00403              boarding1 = false;
00404              led1 = 0;
00405              station1.stop();          
00406         } 
00407                     
00408         if(station2.read() > stationWaitTime){
00409              boarding2 = false;
00410              led2 = 0;
00411              station2.stop();          
00412         }
00413     }
00414     speed1 = boarding1?0:speedsetting;
00415     speed2 = boarding2?0:speedsetting;
00416     
00417     if(next1 == next2 && !collision){
00418         headCollision();
00419         collision = true;
00420     } else if ((next1 == curr2 || next2 == curr1) && !collision){
00421         sideCollision();
00422         collision = true;
00423     } else if (isXCollision() && !collision){
00424         xCollision();
00425         collision = true;
00426     } else if (next1 == prev2 && !command1){
00427         tailCollision(1);
00428         command1 = true;
00429     } else if (next2 == prev1 && !command1){
00430         tailCollision(2);
00431         command1 = true;
00432     } else if (curr1 == 2 && !command1){
00433         atStation2(1);
00434         command1 = true;
00435     } else if (curr1 == 6 && !command1){
00436         atSwitch3(1);
00437         command1 = true;
00438     } else if (curr1 == 8 && !command1){
00439         atSwitch4(1);
00440         command1 = true;
00441     } else if (curr1 == 12 && !command1){
00442         atStation12(1);
00443         command1 = true;
00444     } else if (curr2 == 2 && !command2){
00445         atStation2(1);
00446         command2 = true;
00447     } else if (curr2 == 6 && !command2){
00448         atSwitch3(2);
00449         command2 = true;
00450     } else if (curr2 == 8 && !command2){
00451         atSwitch4(2);
00452         command2 = true;
00453     } else if (curr2 == 12 && !command2){
00454         atStation12(2);
00455         command2 = true;
00456     }
00457     
00458     driveTrain(2);
00459     driveTrain(1);
00460     
00461     /*
00462     else if (!command1 && (curr1 == 9 && next1 == 3) || (curr1 == 5 && next1 == 11)){
00463         loopDeltaCrossing(1);
00464         command1 = true;
00465     } else if (!command2 && (curr2 == 9 && next2 == 3) || (curr2 == 5 && next2 == 11)){
00466         loopDeltaCrossing(2);
00467         command2 = true;
00468     } 
00469     */
00470 }
00471 
00472 bool isXCollision(){
00473     return (curr1 == 3 && next1 == 9) && (next2 == 5 || next2 == 11)
00474         || (curr1 == 5 && next1 == 11) && (next2 == 3 || next2 == 9)
00475         || (curr1 == 9 && next1 == 3) && (next2 == 5 || next2 == 11)
00476         || (curr1 == 11 && next1 == 5) && (next2 == 3 || next2 == 9)
00477         || (curr2 == 3 && next2 == 9) && (next1 == 5 || next1 == 11)
00478         || (curr2 == 5 && next2 == 11) && (next1 == 3 || next1 == 9)
00479         || (curr2 == 9 && next2 == 3) && (next1 == 5 || next1 == 11)
00480         || (curr2 == 11 && next2 == 5) && (next1 == 3 || next1 == 9);
00481 }
00482 
00483 void stopTrain(int train){
00484     if(train == 1){
00485         DCC_send_command(DCCaddress_train1, DCCinst_estop, 10);
00486         pc.printf("STATUS: Train 1 stopped\n\r");
00487     } else { //train == 2
00488         DCC_send_command(DCCaddress_train2, DCCinst_estop, 10);
00489         pc.printf("STATUS: Train 2 stopped\n\r");
00490     }
00491 }
00492 
00493 void reverseTrain(int train){ //always stop before reversing
00494     if(train == 1){
00495         int stupid;
00496         dir1 = !dir1;
00497         stupid = next1;
00498         next1 = curr1;
00499         curr1 = stupid;
00500         prev1 = predictNextMarker(curr1, next1);
00501         pc.printf("STATUS: Train 1 reversed\n\r");
00502     } else { //train == 2
00503         int swap;
00504         dir2 = !dir2;
00505         swap = next2;
00506         next2 = curr2;
00507         curr2 = swap;
00508         prev2 = predictNextMarker(curr2, next2);
00509         pc.printf("STATUS: Train 2 reversed\n\r");
00510     }
00511 }
00512 
00513 void driveTrain(int train){
00514     if(train == 1){
00515         DCC_send_command(DCCaddress_train1, trainInstruction(dir1, light1, speed1), 25);
00516     } else { //train == 2
00517         DCC_send_command(DCCaddress_train2, trainInstruction(dir2, light2, speed2), 25);  
00518     }
00519 }
00520 
00521 void atStation2(int train){
00522     if(train == 1){
00523         if(next1 != 22){
00524             pc.printf("STATUS: train 1 reached switch 2\n\r");
00525             std::time_t current = time(NULL);
00526             if(difftime(current, switch2action) < switchsafety){
00527                 pc.printf("STATUS: Switch 2 on overheat cooldown!\n\r"); 
00528                 return;
00529             }
00530             stopTrain(1);
00531             if(curr2 == 6 || next2 == 6){
00532                 switch2 = true;
00533             } else if(curr2 == 9 || next2 == 9){
00534                 switch2 = false;
00535             } else { //other train far away
00536                 switch2 = rand()%2;
00537             }
00538             changeSwitch2(switch2);
00539             switch2action = time(NULL);
00540         }
00541         atStation(1);
00542     } else { //train == 2
00543         if(next2 != 22){
00544             pc.printf("STATUS: train 2 reached switch 2\n\r");
00545             std::time_t current = time(NULL);
00546             if(difftime(current, switch2action) < switchsafety){
00547                 pc.printf("STATUS: Switch 2 on overheat cooldown!\n\r"); 
00548                 return;
00549             }
00550             stopTrain(2);
00551             if(curr1 == 6 || next1 == 6){
00552                 //switch2 = true;
00553             } else if(curr1 == 9 || next1 == 9){
00554                 //switch2 = false;
00555             } else { //other train far away
00556                 switch2 = rand()%2;
00557             }
00558             changeSwitch2(switch2);
00559             switch2action = time(NULL); 
00560         }
00561         atStation(2);
00562     }
00563 }
00564 
00565 void atStation12(int train){
00566     if(train == 1){
00567         if(next1 != 22){
00568             pc.printf("STATUS: train 1 reached switch 1\n\r");
00569             std::time_t current = time(NULL);
00570             if(difftime(current, switch1action) < switchsafety){
00571                 pc.printf("STATUS: Switch 1 on overheat cooldown!\n\r"); 
00572                 return;
00573             }
00574             stopTrain(1);
00575             if(curr2 == 8 || next2 == 8){
00576                 //switch1 = true;
00577             } else if(curr2 == 5 || next2 == 5){
00578                 //switch1 = false;
00579             } else { //other train far away
00580                 switch1 = rand()%2;
00581             }
00582             changeSwitch1(switch1);
00583             switch1action = time(NULL);
00584         }
00585         atStation(1);
00586     } else { //train == 2
00587         if(next2 != 22){
00588             pc.printf("STATUS: train 2 reached switch 1\n\r");
00589             std::time_t current = time(NULL);
00590             if(difftime(current, switch1action) < switchsafety){
00591                 pc.printf("STATUS: Switch 1 on overheat cooldown!\n\r"); 
00592                 return;
00593             }
00594             stopTrain(2);
00595             if(curr1 == 8 || next1 == 8){
00596                 //switch1 = true;
00597             } else if(curr1 == 5 || next1 == 5){
00598                 //switch1 = false;
00599             } else { //other train far away
00600                 switch1 = rand()%2;
00601             }
00602             changeSwitch1(switch1);
00603             switch1action = time(NULL); 
00604         }
00605         atStation(2);
00606     }
00607 }
00608 
00609 void atStation(int train){
00610     if(train == 1){
00611         pc.printf("STATUS: Train 1 reached station!\n\r");
00612         boxled1 = 1;
00613         speed1 = 0;
00614         station1.start();
00615         boarding1 = true;
00616     } else { //train == 2
00617         pc.printf("STATUS: Train 2 reached station!\n\r");
00618         boxled2 = 1;
00619         speed2 = 0;
00620         station2.start();
00621         boarding2 = true;
00622     }
00623 }
00624 
00625 void atSwitch3(int train){
00626     if(train == 1){
00627         if(next1 == 7){ return; }
00628         pc.printf("STATUS: train 1 reached switch 3\n\r");
00629         std::time_t current = time(NULL);
00630         if(difftime(current, switch3action) < switchsafety){
00631                 pc.printf("STATUS: Switch 3 on overheat cooldown!\n\r"); 
00632                 return;
00633         }
00634         stopTrain(1);
00635         if(curr2 == 2 || next2 == 2){
00636             //switch3 = true; TODO complicated
00637         } else if(curr2 == 11 || next2 == 11){
00638             //switch3 = false;
00639         } else { //other train far away
00640             switch3 = rand()%2;
00641         }
00642         changeSwitch3(switch3);
00643         switch3action = time(NULL);
00644     } else { //train == 2
00645         if(next2 == 7){ return; }
00646         pc.printf("STATUS: train 2 reached switch 3\n\r");
00647         std::time_t current = time(NULL);
00648         if(difftime(current, switch3action) < switchsafety){
00649             pc.printf("STATUS: Switch 3 on overheat cooldown!\n\r"); 
00650             return;
00651         }
00652         stopTrain(2);
00653         if(curr1 == 2 || next1 == 2){
00654             //switch3 = true;
00655         } else if(curr1 == 11 || next1 == 11){
00656             //switch3 = false;
00657         } else { //other train far away
00658             switch3 = rand()%2;
00659         }
00660         changeSwitch3(switch3);
00661         switch3action = time(NULL);
00662     }
00663 }
00664 
00665 void atSwitch4(int train){
00666     if(train == 1){
00667         if(next1 == 7){ return; }
00668         pc.printf("STATUS: train 1 reached switch 4\n\r");
00669         std::time_t current = time(NULL);
00670         if(difftime(current, switch4action) < switchsafety){
00671             pc.printf("STATUS: Switch 4 on overheat cooldown!\n\r"); 
00672             return;
00673         }
00674         stopTrain(1);
00675         if(curr2 == 2 || next2 == 2){
00676             //switch4 = true;
00677         } else if(curr2 == 11 || next2 == 11){
00678             //switch4 = false;
00679         } else { //other train far away
00680             switch4 = rand()%2;
00681         }
00682         changeSwitch4(switch4);
00683         switch4action = time(NULL);
00684     } else { //train == 2
00685         if(next2 == 7){ return; }
00686         pc.printf("STATUS: train 2 reached switch 4\n\r");
00687         std::time_t current = time(NULL);
00688         if(difftime(current, switch4action) < switchsafety){
00689             pc.printf("STATUS: Switch 4 on overheat cooldown!\n\r"); 
00690             return;
00691         }
00692         stopTrain(2);
00693         if(curr1 == 2 || next1 == 2){
00694             //switch4 = true;
00695         } else if(curr1 == 11 || next1 == 11){
00696             //switch4 = false;
00697         } else { //other train far away
00698             switch4 = rand()%2;
00699         }
00700         changeSwitch4(switch4);
00701         switch4action = time(NULL);   
00702     }
00703 }
00704 
00705 void headCollision(void){
00706     buzzer = 1;
00707     pc.printf("WARNING: inbound head collision\n\r");
00708     stopTrain(1);
00709     stopTrain(2);
00710     reverseTrain(1);
00711     reverseTrain(2);
00712     buzzer = 0;
00713 }
00714 
00715 void sideCollision(void){
00716     buzzer = 1;
00717     pc.printf("WARNING: possible side collision\n\r");
00718     stopTrain(1);
00719     stopTrain(2);
00720     reverseTrain(1);
00721     reverseTrain(2);
00722     buzzer = 0;
00723 }
00724 
00725 void xCollision(void){
00726     buzzer = 1;
00727     pc.printf("WARNING: possible cross collision\n\r");
00728     stopTrain(1);
00729     stopTrain(2);
00730     reverseTrain(1);
00731     reverseTrain(2);
00732     buzzer = 0;
00733 }
00734 
00735 void tailCollision(int backTrain){
00736     buzzer = 1;
00737     pc.printf("WARNING: possible tail collision\n\r");
00738     stopTrain(backTrain);
00739     reverseTrain(backTrain);
00740     buzzer = 0;
00741 }
00742 /*
00743 void loopDeltaCrossing(int train){
00744     buzzer = 1;
00745     pc.printf("WARNING: attempting to exit the loop\n\r");
00746     if(train == 1){
00747         if(curr2 != 13 && curr2 != 0 && curr2 != 1 && curr2 != 2){
00748             stopTrain(1);
00749             reverseTrain(1);   
00750         }
00751         //if train 2 in loop
00752             //stop, reverse
00753     } else { //train == 2    
00754         //if train 1 in loop
00755             //stop, reverse
00756         if(curr1 != 13 && curr1 != 0 && curr1 != 1 && curr1 != 2){
00757             stopTrain(1);
00758             reverseTrain(1);   
00759         }
00760     }
00761     buzzer = 0;
00762 }
00763 */
00764 
00765 
00766 
00767 
00768 //TRACK COMMANDS
00769 //code from the tutorial, sends a message to the track through the train pin
00770 void DCC_send_command(unsigned int address, unsigned int inst, unsigned int repeat_count){
00771     switch(address){
00772         case DCCaddress_train1: led1 = 1; break;
00773         case DCCaddress_train2: led2 = 1; break;
00774         case DCCaddress_decoder: led3 = 1; break;    
00775     }
00776     
00777     unsigned __int64 command = 0x0000000000000000; // __int64 is the 64-bit integer type
00778     unsigned __int64 temp_command = 0x0000000000000000;
00779     unsigned __int64 prefix = 0x3FFF; // 14 "1" bits needed at start
00780     unsigned int error = 0x00; //error byte
00781     //calculate error detection byte with xor
00782     error = address ^ inst;
00783     //combine packet bits in basic DCC format
00784     command = (prefix<<28)|(address<<19)|(inst<<10)|((error)<<1)|0x01;
00785     //printf("\n\r %llx \n\r",command);
00786     int i=0;
00787     //repeat DCC command lots of times
00788     while(i < repeat_count) {
00789         temp_command = command;
00790     //loops through packet bits encoding and sending out digital pulses for a DCC command
00791         for (int j=0; j<64; j++) {
00792             if((temp_command&0x8000000000000000)==0) { //test packet bit
00793                 //send data for a "0" bit
00794                 data=0;
00795                 wait_us(100);
00796                 data=1;
00797                 wait_us(100);
00798                 //printf("0011");
00799             } else {
00800                 //send data for a "1"bit
00801                 data=0;
00802                 wait_us(58);
00803                 data=1;
00804                 wait_us(58);
00805                 //printf("01");
00806             }
00807             // next bit in packet
00808             temp_command = temp_command<<1;
00809         }
00810         i++;
00811     }
00812     
00813     led1 = 0;
00814     led2 = 0;
00815     led3 = 0;
00816 }
00817 
00818 void changeSwitch1(bool inwards){
00819     if(inwards){
00820         DCC_send_command(DCCaddress_decoder, DCCswitch1, 1);
00821         pc.printf("STATUS: Switch 1 changed inward\n\r");
00822     } else {
00823         DCC_send_command(DCCaddress_decoder, DCCswitch1, 1);
00824         DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
00825         pc.printf("STATUS: Switch 1 changed outward\n\r");        
00826     }
00827     updateTrainPredictions();
00828 }
00829 
00830 void changeSwitch2(bool inwards){
00831     if(inwards){
00832         DCC_send_command(DCCaddress_decoder, DCCswitch2, 1);
00833         pc.printf("STATUS: Switch 2 changed inward\n\r");
00834     } else {
00835         DCC_send_command(DCCaddress_decoder, DCCswitch2, 1);
00836         DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
00837         pc.printf("STATUS: Switch 2 changed outward\n\r");        
00838     }
00839     updateTrainPredictions();    
00840 }
00841 
00842 void changeSwitch3(bool inwards){
00843     if(inwards){
00844         DCC_send_command(DCCaddress_decoder, DCCswitch3, 1);
00845         DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);  //switch 3 has reverse behavior
00846         pc.printf("STATUS: Switch 3 changed inward\n\r");
00847     } else {
00848         DCC_send_command(DCCaddress_decoder, DCCswitch3, 1);
00849         pc.printf("STATUS: Switch 3 changed outward\n\r");
00850     }
00851     updateTrainPredictions();    
00852 }
00853 
00854 void changeSwitch4(bool inwards){
00855     if(inwards){
00856         DCC_send_command(DCCaddress_decoder, DCCswitch4, 1);
00857         pc.printf("STATUS: Switch 4 changed inward\n\r");
00858     } else {
00859         DCC_send_command(DCCaddress_decoder, DCCswitch4, 1);
00860         DCC_send_command(DCCaddress_decoder, DCCswitchneutral, 1);
00861         pc.printf("STATUS: Switch 4 changed outward\n\r");        
00862     }
00863     updateTrainPredictions();    
00864 }
00865 
00866 void updateTrainPredictions(){  
00867     next1 = predictNextMarker(prev1, curr1);
00868     next2 = predictNextMarker(prev2, curr2);   
00869 }
00870 
00871 unsigned int trainInstruction(bool direction, bool light, int speed){
00872     unsigned int result = 0x40; //0100 0000
00873     if(direction){
00874         result += 0x20;  //0010 0000
00875     } 
00876     if(light){
00877         result += 0x10;  //0001 0000
00878     }
00879     if(speed>0 && speed<15){
00880         result += speed + 1;   
00881     }
00882     return result;   
00883 }
00884 
00885 //INTERPRET INTERRUPT SIGNALS
00886 //INTERRUPTS
00887 //code from the tutorial, initializes MCP and sets it up for interrupts
00888 void init_mcp() {
00889     // Initialisation of MCP registers, documentation on registers is available at
00890     //Niels/Abel/Robert/Natalia
00891     mcp = new MCP23017(i2c, 0x40);
00892     mcp->_write(IODIRA, (unsigned char )0xff);
00893     mcp->_write(IODIRB, (unsigned char )0xff);
00894     mcp->_write(IPOLA, (unsigned char )0x00);
00895     mcp->_write(IPOLB, (unsigned char )0x00);
00896     mcp->_write(DEFVALA, (unsigned char )0xff);
00897     mcp->_write(DEFVALB, (unsigned char )0xff);
00898     mcp->_write(INTCONA, (unsigned char )0xff);
00899     mcp->_write(INTCONB, (unsigned char )0xff);
00900     mcp->_write(IOCONA, (unsigned char )0x2);
00901     mcp->_write(IOCONB, (unsigned char )0x2);
00902     mcp->_write(GPPUA, (unsigned char )0xff);
00903     mcp->_write(GPPUB, (unsigned char )0xff);
00904 
00905     // Clear current interrupts
00906     mcp->_read(GPIOA);
00907     mcp->_read(GPIOB);
00908     // Register callbacks
00909     int0.fall(&on_int0_change);
00910     int1.fall(&on_int1_change);
00911     // Enable interrupts on the MCP
00912     mcp->_write(GPINTENA, (unsigned char )0xff);
00913     mcp->_write(GPINTENB, (unsigned char )0xff);
00914     
00915     int0flag = false;
00916     int1flag = false;
00917 }
00918 
00919 void on_int0_change() {
00920     int0flag = true;
00921 }
00922 
00923 void on_int1_change() {
00924     int1flag = true;
00925 }
00926 
00927 std::list<int> circuit0markers(int sensor_data){
00928     std::list<int> result;
00929     sensor_data = ~sensor_data;
00930     if((sensor_data & 0b00000001) == 0b00000001){
00931         result.push_back(0);
00932     }
00933     if((sensor_data & 0b00000010) == 0b00000010){
00934         result.push_back(1);
00935     }
00936     if((sensor_data & 0b00000100) == 0b00000100){
00937         result.push_back(2);
00938     }
00939     if((sensor_data & 0b00001000) == 0b00001000){
00940         result.push_back(3);
00941     }
00942     if((sensor_data & 0b00010000) == 0b00010000){
00943         result.push_back(4);
00944     }
00945     if((sensor_data & 0b00100000) == 0b00100000){
00946         result.push_back(5);
00947     }
00948     if((sensor_data & 0b01000000) == 0b01000000){
00949         result.push_back(6);
00950     }
00951     if((sensor_data & 0b10000000) == 0b10000000){
00952         result.push_back(7);
00953     }
00954     return result;
00955 }    
00956     
00957 std::list<int> circuit1markers(int sensor_data){
00958     std::list<int> result;
00959     sensor_data = ~sensor_data;
00960     if((sensor_data & 0b00000001) == 0b00000001){
00961         result.push_back(8);
00962     }
00963     if((sensor_data & 0b00000010) == 0b00000010){
00964         result.push_back(9);
00965     }
00966     if((sensor_data & 0b00000100) == 0b00000100){
00967         result.push_back(10);
00968     }
00969     if((sensor_data & 0b00001000) == 0b00001000){
00970         result.push_back(11);
00971     }
00972     if((sensor_data & 0b00010000) == 0b00010000){
00973         result.push_back(12);
00974     }
00975     if((sensor_data & 0b00100000) == 0b00100000){
00976         result.push_back(13);
00977     }
00978     if((sensor_data & 0b01000000) == 0b01000000){
00979         //result.push_back(21); do nothing; sensor 21 badly positioned
00980     }
00981     if((sensor_data & 0b10000000) == 0b10000000){
00982         //result.push_back(22); do nothing; too close
00983     }
00984     return result;
00985 }
00986 
00987 //LOG OUTPUT
00988 void displayLCDstatus(){
00989     lcd.cls();
00990     lcd.printf("Train 1: %d \nTrain 2: %d", speed1, speed2);   
00991 }
00992 
00993 void displayTerminalStatus(){
00994     string dir1Text = dir1?"forward":"reverse";
00995     string dir2Text = dir2?"forward":"reverse";
00996     string light1Text = light1?"on, ":"off,";
00997     string light2Text = light2?"on, ":"off,";
00998     pc.printf("SYSTEM: Iteration %d\n\r", iter);
00999     pc.printf("Train 1: dir %s, light %s spd %d\n\rTrain 2: dir %s, light %s spd %d\n\r", dir1Text.c_str(), light1Text.c_str(), speed1, dir2Text.c_str(), light2Text.c_str(), speed2);   
01000 }