Diff: main.cpp

Revision:

43:346a1f4144cd

Parent:

41:4fa6aa29d1ed

Child:

44:94870081aece

--- a/main.cpp	Thu Jun 14 14:45:54 2018 +0000
+++ b/main.cpp	Mon Jun 18 13:42:48 2018 +0000
@@ -160,14 +160,17 @@
             position = &positions[pos];
             going_cw = cw;
         }
+        Train(bool cw){
+            going_cw = cw;    
+        }
         
         vector<int> get_next_sensors(){
             
             //Checking direction
             if(going_cw){
-                position->get_next_cw();
+                return position->get_next_cw();
             }else{
-                position->get_next_ccw();
+                return position->get_next_ccw();
             }            
         }
         
@@ -211,6 +214,12 @@
 Position d21(D21);
 Position d22(D22);
 
+int area_A_arr[] = {D21,D2,D22,D1,D0,D13,D12};
+int area_B_arr[] = {D6,D7,D8};
+
+const vector<int> area_A(area_A_arr,area_A_arr + sizeof(area_A_arr) / sizeof(int));
+const vector<int> area_B(area_B_arr,area_B_arr + sizeof(area_B_arr) / sizeof(int));
+
 
 
 
@@ -248,8 +257,11 @@
 
 
 //Starting position and orientation of the trains
-Train DR_train(D4,true); //Position and going_cw
-Train LR_train(D10,true);
+Train DR_train(true); //Position and going_cw
+Train LR_train(true);
+
+bool DR_run = true;
+bool LR_run = true;
 
 
 //**************** FUNCTIONS FOR DENVER TRAIN ****************//
@@ -383,30 +395,128 @@
     return sensor;
 }
 
+bool in_vector(vector<int>v,int element){
+    bool exist = false;
+    
+    for(int i=0; i< v.size(); i++){
+        if(v[i] == element){
+            exist = true;
+        }
+    }
+    
+    return exist;
+}
+
+/**
+* Check if there is an avodable frontal collision:
+* Both trains in area A or B with different direction
+* Trains in (D11 and D5) or (D9 and D3) with same direction 
+*/
+bool check_NAC(bool DR_in_A, bool DR_in_B,bool LR_in_A,bool LR_in_B){
+    bool NAC = false;
+    
+    if((DR_in_A && LR_in_A) || (DR_in_B && LR_in_B) ){ //Check if both are in same area
+        if(DR_train.goes_cw() ^ LR_train.goes_cw()){ //XOR: They must have different values to be true (Different direction)
+            NAC = true;
+        }
+    }else if((DR_train.get_position_number() == D11) && (LR_train.get_position_number() == D5 )){ //Check if they are in position D11 and D5
+        if(!(DR_train.goes_cw() ^ LR_train.goes_cw())){ // NOT XOR: They must have same values to be true (Same direction)
+            NAC = true;
+        }
+    }else if((DR_train.get_position_number() == D9) && (LR_train.get_position_number() == D3 )){//Check if they are in position D9 and D3
+        if(!(DR_train.goes_cw() ^ LR_train.goes_cw())){ // NOT XOR: They must have same values to be true (Same direction)
+            NAC = true;
+        }   
+    }
+    
+    return NAC;    
+    
+}
+
+/**
+* Check if there is an Avoidable Frontal Collision
+* Train in area A(ccw) and train in D4(cw)
+* Train in area A(cw) and train in D10(ccw)
+* Train in area B(cw) and train in D4(ccw)
+* Train in area B(ccw) and train in D10(ccw) 
+*/
+bool check_AFC(bool DR_in_A, bool DR_in_B,bool LR_in_A,bool LR_in_B){
+    if( DR_train.get_position_number() == D4){           
+            if(DR_train.goes_cw()){
+                if(LR_in_A && !LR_train.goes_cw()){
+                //Activate switch2
+                //DR_train has to stop
+                //When LR is at D3 DR continues
+                }
+            }else{ //DR goes ccw
+                if(LR_in_B && LR_train.goes_cw()){
+                    //DR_train stops
+                    //Activate switch3
+                    //When LR is at D5 DR continues
+                }
+            }
+            
+    }else if(DR_train.get_position_number() == D10){
+        if(DR_train.goes_cw()){
+            if(LR_in_B && !LR_train.goes_cw()){
+                //DR train stops
+                //Activate switch4
+                //When LR is at D9 DR continues
+            }
+        }else{
+            if(LR_in_A && LR_train.goes_cw()){
+                //DR train stops
+                //Activate switch1
+                //When LR is at D9 DR continues
+            }
+        }
+    }
+}
+
+void check_position(){
+    bool DR_in_A, DR_in_B, LR_in_A, LR_in_B;
+    
+    DR_in_A=in_vector(area_A,DR_train.get_position_number()); //Check if DR train is in area A
+    DR_in_B=in_vector(area_B,DR_train.get_position_number());
+    LR_in_A=in_vector(area_A,LR_train.get_position_number());
+    LR_in_B=in_vector(area_B,LR_train.get_position_number());      
+    
+}
+
 void update_train_pos(int sensor){
     
     bool found_DR = false;
     bool found_LR = false;
     
     lcd.cls();
-    lcd.printf("Sensor D%d DR(%d) LR(%d)",sensor,DR_train.get_next_sensors().size(),LR_train.get_next_sensors().size());    
+    lcd.printf("Sensor D%d \n DR(",sensor);
+    
+    //TODO: Do a for to print all next sensors.
+    for(int i=0; i<DR_train.get_next_sensors().size(); i++){ 
+        lcd.printf("%d,",DR_train.get_next_sensors()[i]);
+    }
+    lcd.printf(") LR(");
+    
+    for(int i=0; i<LR_train.get_next_sensors().size(); i++){    
+        lcd.printf("%d,",LR_train.get_next_sensors()[i]);
+    }
+    lcd.printf(")");
+           
+    wait(0.7);   
     
     //Checking next sensors for DR train
-    for(int i=0; i<DR_train.get_next_sensors().size(); i++){
-        lcd.cls();
-        lcd.printf("I am inside the for :D",sensor); 
-        lcd.printf("Detected!"); 
+    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;
             DR_train.set_position(sensor);
             
             if(DR_train.goes_cw()){
-                if(sensor == D5 || sensor == D11){
+                if(sensor == D9 || sensor == D3){                
                     DR_train.set_goes_cw(false); //If train goes cw and passes D5 or D11 we change orientation
                 }    
             }else{
-                if(sensor == D9 || sensor == D3){
+                if(sensor == D5 || sensor == D11){
                     DR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
                 }
             }                
@@ -416,20 +526,20 @@
     
     //Checking next sensors for LR train
     for(int i=0; i<LR_train.get_next_sensors().size(); i++){
-        lcd.cls();
-        lcd.printf("I am inside the for :D",sensor); 
+        
         if(LR_train.get_next_sensors()[i] == sensor){
-            
+            lcd.cls();
             lcd.printf("Detected!"); 
             found_LR = true;
             LR_train.set_position(sensor);
             
-            if(LR_train.goes_cw()){
-                if(sensor == D5 || sensor == D11){
+            if(LR_train.goes_cw()){                
+                if(sensor == D9 || sensor == D3){
                     LR_train.set_goes_cw(false); //If train goes cw and passes D5 or D11 we change orientation
                 }    
             }else{
-                if(sensor == D9 || sensor == D3){
+                    
+                if(sensor == D5 || sensor == D11){
                     LR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
                 }
             }  
@@ -633,7 +743,19 @@
             }
     }
 
-
+void send_command(){
+    if(DR_run){
+        DCC_send_command(DCCaddressDR,DCCinst_forward,1); // Forward half speed train addres DARK-RED 
+    }else{
+        DCC_send_command(DCCaddressDR,DCCinst_stop,400);
+    }
+    
+    if(LR_run){
+        DCC_send_command(DCCaddressLR,DCCinst_forward,1); // Forward half speed train addres DARK-RED 
+    }else{
+        DCC_send_command(DCCaddressLR,DCCinst_stop,400);
+    }
+}
 
 //**************** MAIN PROGRAM FOR DENVER TRAIN ****************//
 
@@ -662,10 +784,15 @@
     init();
     init_positions();
     
+    DR_train.set_position(D4);
+    LR_train.set_position(D10);
+    
     //Train light routine to start running
+    /*
     DCC_send_command(DCCaddressDR,DCC_func_lighton,200); // turn light on full
     DCC_send_command(DCCaddressDR,DCC_func_dimlight,400); //dim light
     DCC_send_command(DCCaddressDR,DCC_func_lighton,200);  //light full again
+    */
 
     //LED3 Shows start of route + LCD notif
     led3 = 1; // Entering the while
@@ -686,8 +813,7 @@
             lcd.cls();
             
         }else{
-            DCC_send_command(DCCaddressDR,DCCinst_forward,1); // Forward half speed train addres DARK-RED             
-            DCC_send_command(DCCaddressLR,DCCinst_forward,1); // Forward half speed train address LIGHT-RED
+            send_command();
         } 
     }
 }