Diff: main.cpp

Revision:

55:aa3baa01f43d

Parent:

52:c08495446f87

Child:

56:fe999806787d

--- a/main.cpp	Wed Jun 20 12:54:28 2018 +0000
+++ b/main.cpp	Thu Jun 21 12:19:29 2018 +0000
@@ -91,6 +91,8 @@
 #define D22 15
 
 
+
+
 /**
 *
 *Position class.
@@ -173,6 +175,7 @@
 class Train{
     
     private:
+        int mode;
         Position *position;
         bool going_cw;
         
@@ -295,7 +298,7 @@
 *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(false); //Position and going_cw
+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
@@ -518,10 +521,24 @@
     return NAC;     
 }
 
+/*
+void AFC_action(int switch_n, int sensor, Train *stop_train, Train * cont_train ){
+    
+    flip_switch(switch_n,5); //Activate switch 5
+    
+    while(cont_train->get_position_number() != sensor){
+        DR_run = false; //DR has to stop
+        LR_run = true; //LR continues
+        send_command();
+    }
+    DR_run = true;
+    LR_run = true; //Both trains start to run again
+}
+*/
 
 /**
 *
-*CThe function will check if there is an Avoidable Frontal Collision (AFC).
+*The function will check if there is an Avoidable Frontal Collision (AFC).
 *AFC will occur if:
 *
 *Train in area A(ccw) and train in D4(cw)
@@ -588,7 +605,12 @@
     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());      
+    LR_in_B = in_vector(area_B,LR_train.get_position_number());
+    
+    if(check_NAC(DR_in_A,DR_in_B,LR_in_A,LR_in_B)){
+        lcd.cls();
+        lcd.printf("NAC!!!");
+    }      
     
 }
 
@@ -618,106 +640,105 @@
         LR_dir = "cw";
     }else{
         LR_dir = "ccw";
-    }
+    }    
     
     
-    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++){ 
-    
-        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);
     //wait(0.7);   
     
-    if(sensor == DR_train.get_position_number()){
-        led2 = 0;
-        //lcd.cls();
-        //lcd.printf("Sensor TWICE!!");
+    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++){ 
+        
+            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;
-          
+        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
             
-            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{
+                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){
                 
-                if(DR_train.get_position_number() == D9 || DR_train.get_position_number() == D3){ 
+                found_LR = true;
+                            
+                if(LR_train.goes_cw()){   
                     
-                    DR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
+                    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){
             
-            DR_train.set_position(sensor);
-                           
+            //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 :(");
+        }
+        */
+        
     }
     
-    //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 :(");
-    }
-    */
 }
 
 
@@ -731,8 +752,8 @@
      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);
+     //lcd.cls();
+     //lcd.printf("int0 0x%x \n Sensor: %d",sensor_data,sensor);
       
      update_train_pos(sensor);
 }
@@ -748,8 +769,8 @@
     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);
+     //lcd.cls();
+     //lcd.printf("int1 0x%x \n Sensor: %d",sensor_data,sensor);
      
      update_train_pos(sensor);
 }
@@ -835,7 +856,7 @@
 *@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){
+void flip_switch(int switchId, int times, bool activate=true){
     
     unsigned int SWBflip = SWBidle; //IDLE - Flip last activated SW.
     
@@ -885,22 +906,22 @@
         
         lcd.cls();
         lcd.printf("Switch 1 ON - SW1");
-        flipSwitch(1,5);
+        flip_switch(1,5);
      }else if(switch2 == 1){
         
             lcd.cls();
             lcd.printf("Switch 2 ON - SW2");
-            flipSwitch(2,5);
+            flip_switch(2,5);
         }else if(switch3 == 0){
                 
                 lcd.cls();
                 lcd.printf("Switch 3 ON - SW3");
-                flipSwitch(3,5); 
+                flip_switch(3,5); 
             }else if(switch4 == 0){
         
                     lcd.cls();
                     lcd.printf("Switch 4 ON - IDLE");
-                    flipSwitch(0,5);         
+                    flip_switch(0,5);         
             }
     }
 
@@ -929,8 +950,19 @@
     }
 }
 
-int select_sensor(){
-    int sensor = 0;
+
+/**
+* Returns a sensor number depending on how many times switch3 flips.
+* When pressing switch4 it confirms the switch
+* Init_sensor is the value where we start counting. 
+* string train is the name of the train that will be prited with the sensor
+*/
+int select_sensor(int init_sensor, string train){
+    
+    lcd.cls();
+    lcd.printf("%s SENSOR",train);
+    
+    int sensor = init_sensor;
     bool changed = false;
     bool exit = false;
     
@@ -940,20 +972,66 @@
                sensor++;
                sensor=sensor%15; //Only sensors from 0 to 15.
                changed=false; 
+               lcd.cls();
+               lcd.printf("%s: D%d",train,sensor);
             }
             
         }else{
             changed = true;
+            wait(0.2);
         }
         
         if(switch4 == 0){
             exit = true;
+            wait(0.2);
         }
     }
     
     return sensor;
 }
 
+/**
+* Returns a boolean representing the direction. Everytimew switch3 is 0 it changes the direction.
+* When switch4 is 0 the selection is confirmed.
+* Init_going_cw is the initial direction.
+* Train is the string with the name of the train  that will be printed next to the direction
+*/
+bool select_direction(bool init_going_cw,string train){
+    
+    lcd.cls();
+    lcd.printf("%s DIRECTION",train);
+    bool exit = false;
+    bool going_cw = init_going_cw;
+    bool changed = false;
+    
+    while(!exit){
+        if(switch3 == 0){
+            if(changed){
+                going_cw = !going_cw;
+                changed = false;
+                string dir;
+                if(going_cw){
+                    dir = "cw";
+                }else{
+                    dir = "ccw";
+                }
+                lcd.cls();
+                lcd.printf("%s: %s",train,dir);
+            }
+        }else{
+            changed = true;
+            wait(0.2);
+        }
+        
+        if(switch4 == 0){
+            exit = true;
+            wait(0.2);
+        }
+    }
+    
+    return going_cw;
+}
+
 //**************** MAIN PROGRAM FOR DENVER TRAIN ****************//
 
 
@@ -979,12 +1057,47 @@
     wait(0.2);
     led1 = 1;
     
+    init_positions();
+    
     initialize_mcp();   //mcp initialization for interrupts before train running
     init();
-    init_positions();
+    
+    
+    
+    int DR_init_sensor = select_sensor(D9,"DR");
+    bool DR_init_dir = select_direction(false,"DR");
+    
+    wait(0.5);
+    
+    int LR_init_sensor = select_sensor(D9,"LR");
+    bool LR_init_dir = select_direction(true,"LR");
+    
+    DR_train.set_position(DR_init_sensor);
+    DR_train.set_goes_cw(DR_init_dir);
+    
+    LR_train.set_position(LR_init_sensor);
+    LR_train.set_goes_cw(LR_init_dir);
+    
     
-    DR_train.set_position(D10);
-    LR_train.set_position(D4);
+    string DR_print_dir, LR_print_dir;
+    
+    if(DR_train.goes_cw()){
+        DR_print_dir = "cw";
+    }else{
+        DR_print_dir = "ccw";
+    }
+    
+    if(LR_train.goes_cw()){
+        LR_print_dir = "cw";
+    }else{
+        LR_print_dir = "ccw";
+    }
+    
+    lcd.cls();
+    lcd.printf("DR(%d)%s \n LR(%d)%s",DR_train.get_position_number(),DR_print_dir,LR_train.get_position_number(),LR_print_dir);
+    
+    wait(2);
+    
     
     //Train light routine to start running
     /*
@@ -997,11 +1110,13 @@
     led3 = 1; // Entering the while
     lcd.cls();
     lcd.printf("Ready to start");
+    wait(1);
     
     //Demo for stopping at the station
     while(1) {
         
-        checkSwitch();  //Checks for switch commands everytime.  
+        checkSwitch();  //Checks for switch commands everytime. 
+        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.
@@ -1013,6 +1128,7 @@
             
         }else{
             send_command();
+            
         } 
     }
 }