Diff: main.cpp

Revision:

71:9a303d90c9b8

Parent:

70:594fe5c1ad5c

Child:

72:a1bd95af46b8

--- a/main.cpp	Wed Jun 27 09:22:45 2018 +0000
+++ b/main.cpp	Wed Jun 27 09:53:49 2018 +0000
@@ -70,7 +70,6 @@
 //MCP
 MCP23017 *mcp;
 
-//------
 
 
 //------GLOBAL VARS
@@ -642,18 +641,8 @@
             break;    
     }          
 
-    //Security measure not to burn the switch.
-    
-    
-        DCC_send_command(SWBaddress,SWBflip,times); //Activating switchç
-        
-        /*
-        if(switchId == 3){
-             DCC_send_command(SWBaddress,SWBidle,times); //Activating switch
-        }
-        */
-        
-        
+        //Security measure not to burn the switch.
+        DCC_send_command(SWBaddress,SWBflip,times); //Activating switch         
         
         if(!activate){
               if(switchId == 3){
@@ -665,6 +654,7 @@
      
 }
 
+
 /**
 * Action to do when NAC is detected 
 * Booster is disabled and the buzz is activated
@@ -678,13 +668,14 @@
 
 
 /**
+*
 *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;
@@ -714,22 +705,24 @@
     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;
-    
-    //flip_switch(switch_n,5);
-    
+        
     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);
@@ -737,6 +730,7 @@
         cont_train->run();
         
         if(send_pack_switch){
+            
             lcd.cls();
             lcd.printf("Switching SW%d",switch_n);
             flip_switch(switch_n,5);
@@ -744,32 +738,31 @@
     }
     
     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 ){
-    
-    //flip_switch(switch_n,5);
-    
+        
     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);
 }
 
@@ -796,19 +789,19 @@
                 
                 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
-                
-                detected_AFC = true;
                 }
             }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");
                     
@@ -816,15 +809,15 @@
                     //DR_train stops
                     //Activate switch3
                     //When LR is at D5 DR continues
-                    detected_AFC = true;
                 }
             }
-            
     }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");
                 
@@ -832,18 +825,18 @@
                 //DR train stops
                 //Activate switch4
                 //When LR is at D9 DR continues
-                detected_AFC = true;
             }
         }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
-                detected_AFC = true;
             }
         }
     }else if(stop_train->get_position_number() == D9){
@@ -852,42 +845,40 @@
             
             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
-                detected_AFC = true;
+                //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
-                detected_AFC = true;
             }
         }
-        
     }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
-                detected_AFC = true;
             }
         }
         
@@ -897,12 +888,12 @@
             
             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
-                detected_AFC = true;
             }
         }
     }
@@ -918,63 +909,57 @@
 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
-                
-                detected_ALC = true;
                 }
             }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
-                    detected_ALC = true;
                 }
             }
-            
     }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
-                detected_ALC = true;
             }
         }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
-                detected_ALC = true;
             }
         }
     }
-    
     return detected_ALC;
 }
 
@@ -988,17 +973,15 @@
 void check_position(){    
         
     if(check_NAC()){
-        //lcd.cls();
-        //lcd.printf("NAC!!!");
+        lcd.cls();
+        lcd.printf("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);
-
-            
-    
 }
 
 
@@ -1011,54 +994,23 @@
 **/
 void update_train_pos(int sensor){
  
-    bool found_DR = false;
-    bool found_LR = false;
-    
-    string DR_dir,LR_dir;
+    //bool found_DR = false;
+    //bool found_LR = false;
     
-    if(DR_train.goes_cw()){
-        DR_dir = "cw";
-    }else{
-        DR_dir = "ccw";
-    }
-    
-    if(LR_train.goes_cw()){
-        LR_dir = "cw";
-    }else{
-        LR_dir = "ccw";
-    }    
-       
-    
+
     if(sensor == DR_train.get_position_number() || sensor == LR_train.get_position_number()){
-        led2 = 0;        
-    }else{
-                
-        lcd.cls();
-        lcd.printf("S:D%d DR%d(",sensor,DR_train.get_position_number());
-        
-        //TODO: Do a for to print all next sensors.
-        for(int i=0; i<DR_train.get_next_sensors().size(); i++){ 
+        redled = 1;        
+    }else{  
+
+        redled = 0;
+        printPos();
         
-            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);
-        
-
         //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;
-              
+                //found_DR = true;
                 
                 if(DR_train.goes_cw()){
                     if(DR_train.get_position_number() == D5 || DR_train.get_position_number() == D11){             
@@ -1083,7 +1035,7 @@
             
             if(LR_train.get_next_sensors()[i] == sensor){
                 
-                found_LR = true;
+                //found_LR = true;
                             
                 if(LR_train.goes_cw()){   
                     
@@ -1097,33 +1049,43 @@
                         LR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
                     }
                 }
-                LR_train.set_position(sensor);
-                 
+                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){
+    }
+}
+
+
+/**
+*
+*
+*
+**/
+void printPos(){
+   
+    string DR_dir,LR_dir;
+    
+    if(DR_train.goes_cw()){ DR_dir = "cw";}
+        else{DR_dir = "ccw";}
     
-            lcd.cls();
-            lcd.printf("LR is at D%d",LR_train.get_position_number());   
-        }
-        
-        if(!found_DR && !found_LR){
+    if(LR_train.goes_cw()){LR_dir = "cw";}
+        else{LR_dir = "ccw";}    
             
-            lcd.cls();
-            lcd.printf("No train before :(");
-        }
-        */
+    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);    
 }
 
 
@@ -1137,8 +1099,6 @@
      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);
 }
 
@@ -1153,12 +1113,10 @@
     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 
@@ -1175,10 +1133,6 @@
   }
 
 
-
-
-
-
 /**
 *
 *Checks if any of the switches of the box has been activated. 
@@ -1225,10 +1179,12 @@
 
 
 /**
+*
 * 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){
     
@@ -1240,26 +1196,28 @@
     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{
             
-        }else{
             changed = true;
             wait(0.2);
         }
-        
         if(switch4 == 0){
+            
             exit = true;
             wait(0.2);
         }
     }
-    
     return sensor;
 }
 
@@ -1274,11 +1232,8 @@
     
     string dir_string;
     
-    if(init_going_cw){
-        dir_string = "cw";
-    }else{
-        dir_string = "ccw";
-    }
+    if(init_going_cw){dir_string = "cw";}
+        else{dir_string = "ccw";}
     
     lcd.cls();
     lcd.printf("%s DIRECTION %s ",train,dir_string);
@@ -1288,30 +1243,32 @@
     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";
-                }
+                
+                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;
 }
 
@@ -1325,8 +1282,6 @@
     
     float f = pot.read();
     float vin = f * 3.3;
-    lcd.cls();
-   // lcd.printf("vin: %.4f",vin);
     
     if(0<=vin && vin<0.60){
         
@@ -1347,6 +1302,11 @@
             }
 }
 
+/**
+*
+*
+*
+**/
 void switch_toSpeed(){
     
     switch(speedcheck){
@@ -1360,37 +1320,24 @@
     }
 }
 
+
+
 //**************** 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
     
     enable = 0;
     
     //Led routine to start main program
-    led1 = 1;
-    wait(0.2);
-    led1 = 0;
     wait(0.2);
     led1 = 1;
     
     init_positions();
-    
     initialize_mcp();   //mcp initialization for interrupts before train running
     init();
        
-    
     int DR_init_sensor = select_sensor(D6,"DR");
     bool DR_init_dir = select_direction(true,"DR");
     
@@ -1405,46 +1352,52 @@
     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(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";
-    }
+    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);
     
-    
-    //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
+    //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) {
+        
+        checkSwitch();  //Checks for switch commands everytime. 
+        switch_toSpeed();
+        lcd.cls();
+        check_position(); 
+        
+}
+
+
+
+//**********************SAVED CODE CHUNKS****************************//
+
+
+/**Flip switch-idle routine
+
     flip_switch(5,15);//Send IDLE command at the beginning
     flip_switch(1,40);
     wait(0.5);
@@ -1458,20 +1411,12 @@
     flip_switch(4,15);
     wait(0.5);
     flip_switch(5,15);
-    */
     
-    
-    //Demo for stopping at the station
-    while(1) {
-        
+**/
 
-        checkSwitch();  //Checks for switch commands everytime. 
-        switch_toSpeed();
-        lcd.cls();
-        check_position(); 
-        
-        if(1==0){
-        //if(station == 1){      //If train is on the sensor at the middle of the station it stops and displays LCD text.
+/**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");
@@ -1483,5 +1428,67 @@
             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
+    
+**/
\ No newline at end of file