Diff: main.cpp

Revision:

41:4fa6aa29d1ed

Parent:

40:9acc1341456a

Child:

42:b445252a772a

Child:

43:346a1f4144cd

--- a/main.cpp	Thu Jun 14 13:19:52 2018 +0000
+++ b/main.cpp	Thu Jun 14 14:45:54 2018 +0000
@@ -88,6 +88,8 @@
 #define D21 14
 #define D22 15
 
+
+
 /**
 *
 *Position class.
@@ -105,6 +107,8 @@
 *add_ccw() - 
 *
 **/
+
+
 class Position{
     private:
         int position; 
@@ -118,6 +122,14 @@
         int get_pos(){
             return position;    
         }
+        
+        vector<int> get_next_cw(){
+            return previous_ccw;
+        }
+        
+        vector<int> get_next_ccw(){
+            return previous_cw;
+        }
 
         vector <int> get_prev_cw(){
             return previous_cw;    
@@ -136,6 +148,50 @@
         };
 };
 
+//Creating a vector with all the positions.
+vector<Position> positions;
+
+class Train{
+    private:
+        Position *position;
+        bool going_cw;
+    public:
+        Train(int pos, bool cw){
+            position = &positions[pos];
+            going_cw = cw;
+        }
+        
+        vector<int> get_next_sensors(){
+            
+            //Checking direction
+            if(going_cw){
+                position->get_next_cw();
+            }else{
+                position->get_next_ccw();
+            }            
+        }
+        
+        void set_position(int pos){
+            position = &positions[pos]; //Taking the new position from the positions vector
+        }
+        
+        void set_goes_cw(bool cw){
+            going_cw = cw;
+        }   
+        
+        Position get_position(){
+            return *position;
+        }
+        
+        int get_position_number(){
+            return position->get_pos();
+        }
+        
+        bool goes_cw(){
+            return going_cw;
+        }
+};
+
 //Creation of all the positions. One for every sensor on the table - Position name(mapping)
 
 Position d0(D0);
@@ -155,8 +211,7 @@
 Position d21(D21);
 Position d22(D22);
 
-//Creating a vector with all the positions.
-vector<Position> positions;
+
 
 
 //.....DCC TRAIN COMMAND VARS
@@ -193,12 +248,8 @@
 
 
 //Starting position and orientation of the trains
-int DR_train_pos = D4;
-bool DR_cw = true;
-
-int LR_train_pos= D10;
-bool LR_cw = true;
-
+Train DR_train(D4,true); //Position and going_cw
+Train LR_train(D10,true);
 
 
 //**************** FUNCTIONS FOR DENVER TRAIN ****************//
@@ -333,35 +384,69 @@
 }
 
 void update_train_pos(int sensor){
+    
     bool found_DR = false;
-    bool found_LR = false;    
-            
-    Position pos = positions[sensor];
-    for(int i = 0; i<pos.get_prev_cw().size();i++){
-            int prev = pos.get_prev_cw()[i];
+    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());    
+    
+    //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!"); 
+        
+        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_pos == prev){
-              
-              found_DR = true;
-              DR_train_pos = sensor; //We update the position of the train  
-            }
+            if(DR_train.goes_cw()){
+                if(sensor == D5 || sensor == D11){
+                    DR_train.set_goes_cw(false); //If train goes cw and passes D5 or D11 we change orientation
+                }    
+            }else{
+                if(sensor == D9 || sensor == D3){
+                    DR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
+                }
+            }                
             
-            if(LR_train_pos == prev){
-                found_LR = true;
-                LR_train_pos = sensor;   //We update the position of the train  
-            }
-     }
+        }
+    }
+    
+    //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.printf("Detected!"); 
+            found_LR = true;
+            LR_train.set_position(sensor);
+            
+            if(LR_train.goes_cw()){
+                if(sensor == D5 || sensor == D11){
+                    LR_train.set_goes_cw(false); //If train goes cw and passes D5 or D11 we change orientation
+                }    
+            }else{
+                if(sensor == D9 || sensor == D3){
+                    LR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
+                }
+            }  
+        }
+    }
+                   
      
      if(found_DR){
         //doBuzz();
         lcd.cls();
-        lcd.printf("DR is at D%d",DR_train_pos);
+        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_pos);   
+        lcd.printf("LR is at D%d",LR_train.get_position_number());   
     }
     if(!found_DR && !found_LR){
         lcd.cls();
@@ -382,8 +467,7 @@
      int sensor = get_sensor(sensor_data,0);
      lcd.cls();
      lcd.printf("int0 0x%x \n Sensor: %d",sensor_data,sensor);
-     
-     
+      
      
      update_train_pos(sensor);
 }