Diff: main.cpp

Revision:

36:9428c72bdd58

Parent:

35:cfcfeccb959e

Child:

38:b9aba3715682

--- a/main.cpp	Thu Jun 14 10:14:59 2018 +0000
+++ b/main.cpp	Thu Jun 14 12:56:31 2018 +0000
@@ -142,6 +142,10 @@
         Position(int p){
             position = p;
         }
+        
+        int get_pos(){
+            return position;    
+        }
 
         vector <int> get_prev_cw(){
             return previous_cw;    
@@ -198,24 +202,7 @@
 }
 
 void init_positions(){
-    //Initialize array with positions
-    positions.push_back(d0);
-    positions.push_back(d1);
-    positions.push_back(d2);
-    positions.push_back(d3);
-    positions.push_back(d4);
-    positions.push_back(d5);
-    positions.push_back(d6);
-    positions.push_back(d7);
-    positions.push_back(d8);
-    positions.push_back(d9);
-    positions.push_back(d10);
-    positions.push_back(d11);    
-    positions.push_back(d12);
-    positions.push_back(d13);
-    positions.push_back(d21);
-    positions.push_back(d22);
-    
+      
     
     d0.add_prev_cw(D1);
     d0.add_prev_ccw(D13);
@@ -249,10 +236,10 @@
     d7.add_prev_cw(D8);
     d7.add_prev_ccw(D6);
     
-    d8.add_prev_cw(D7);
-    d8.add_prev_ccw(D9);
-    d8.add_prev_ccw(D10);
-    
+    d8.add_prev_cw(D9);
+    d8.add_prev_cw(D10);
+    d8.add_prev_ccw(D7);
+        
     d9.add_prev_cw(D3);
     d9.add_prev_ccw(D8);
     
@@ -268,6 +255,24 @@
     
     d13.add_prev_cw(D0);
     d13.add_prev_ccw(D12);  
+    
+    //Initialize array with positions
+    positions.push_back(d0);
+    positions.push_back(d1);
+    positions.push_back(d2);
+    positions.push_back(d3);
+    positions.push_back(d4);
+    positions.push_back(d5);
+    positions.push_back(d6);
+    positions.push_back(d7);
+    positions.push_back(d8);
+    positions.push_back(d9);
+    positions.push_back(d10);
+    positions.push_back(d11);    
+    positions.push_back(d12);
+    positions.push_back(d13);
+    positions.push_back(d21);
+    positions.push_back(d22);
 }
 
 /**
@@ -315,28 +320,40 @@
 
 void update_train_pos(int sensor){
     bool found_DR = false;
-    bool found_LR = false;
+    bool found_LR = false;    
+    
     
      Position pos = positions[sensor];
+     
+     lcd.cls();
+     lcd.printf("D%d DR:%d",pos.get_pos(),DR_train_pos);      
+     //wait(1);   
      for(int i = 0; i<pos.get_prev_cw().size();i++){
+            
             int prev = pos.get_prev_cw()[i];
+            
             if(DR_train_pos == prev){
+              
               found_DR = true;
-              DR_train_pos = prev; //We update the position of the train  
+              DR_train_pos = sensor; //We update the position of the train  
             }
             if(LR_train_pos == prev){
                 found_LR = true;
-                LR_train_pos = prev;   //We update the position of the train  
+                LR_train_pos = sensor;   //We update the position of the train  
             }
      }
      
      if(found_DR){
+        //doBuzz();
         lcd.cls();
         lcd.printf("DR is at D%d",DR_train_pos);
+        
     }
     if(found_LR){
-          lcd.cls();
-        lcd.printf("LR is at D%d",LR_train_pos);   
+        //doBuzz();
+        lcd.cls();
+        lcd.printf("LR is at D%d",LR_train_pos); 
+          
     }
     if(!found_DR && !found_LR){
         lcd.cls();
@@ -357,6 +374,8 @@
      lcd.cls();
      lcd.printf("int0 0x%x \n Sensor: %d",sensor_data,sensor);
      
+     
+     
      update_train_pos(sensor);
 }