Diff: main.cpp

Revision:

35:cfcfeccb959e

Parent:

34:c9ab2a987734

Child:

36:9428c72bdd58

Child:

37:bb15bea420a3

--- a/main.cpp	Wed Jun 13 13:36:14 2018 +0000
+++ b/main.cpp	Thu Jun 14 10:14:59 2018 +0000
@@ -3,6 +3,9 @@
 #include "MCP23017.h"
 #include <string>
 #include <iostream>
+#include <vector>
+
+using namespace std;
 
 //mbed DCC Model Train Demo
  
@@ -26,9 +29,9 @@
 ///p12
 
 //M0 - p13
-DigitalIn d21(p13);     //Sensor right of the station
+DigitalIn d21stat(p13);     //Sensor right of the station
 //M1 - p14
-DigitalIn d22(p14);     //Sensor left of the station
+DigitalIn d22stat(p14);     //Sensor left of the station
 //M2 - p15
 DigitalIn station(p15); //Sensor in the middle of the station
 
@@ -130,6 +133,59 @@
 
 //**************** FUNCTIONS FOR DENVER TRAIN ****************//
 
+class Position{
+    private:
+        int position; 
+        vector <int> previous_cw;
+        vector <int> previous_ccw;
+    public:
+        Position(int p){
+            position = p;
+        }
+
+        vector <int> get_prev_cw(){
+            return previous_cw;    
+        }
+        
+        vector <int> get_prev_ccw(){
+            return previous_ccw;    
+        }
+        
+        void add_prev_cw(int pos){
+            previous_cw.push_back(pos);
+        };
+        
+        void add_prev_ccw(int pos){
+            previous_ccw.push_back(pos);
+        };
+};
+
+Position d0(D0);
+Position d1(D1);
+Position d2(D2);
+Position d3(D3);
+Position d4(D4);
+Position d5(D5);
+Position d6(D6);
+Position d7(D7);
+Position d8(D8);
+Position d9(D9);
+Position d10(D10);
+Position d11(D11);
+Position d12(D12);
+Position d13(D13);
+Position d21(D21);
+Position d22(D22);
+
+vector<Position> positions;
+
+//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;
+
 
 /**
 *Activates the buzzer for 0.5 seconds.
@@ -141,6 +197,78 @@
     buzz = 0;    
 }
 
+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);
+    
+    d1.add_prev_cw(D22);
+    d1.add_prev_ccw(D0);
+    
+    d22.add_prev_cw(D2);
+    d22.add_prev_ccw(D1);
+    
+    d2.add_prev_cw(D21);
+    d2.add_prev_ccw(D22);
+    
+    d21.add_prev_cw(D3);
+    d21.add_prev_cw(D4);
+    d21.add_prev_ccw(D2);
+    
+    d3.add_prev_cw(D9);
+    d3.add_prev_ccw(D21);
+    
+    d4.add_prev_cw(D6);
+    d4.add_prev_ccw(D21);
+    
+    d5.add_prev_cw(D6);
+    d5.add_prev_ccw(D11); 
+    
+    d6.add_prev_cw(D7);
+    d6.add_prev_ccw(D4);
+    d6.add_prev_ccw(D5);
+    
+    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);
+    
+    d9.add_prev_cw(D3);
+    d9.add_prev_ccw(D8);
+    
+    d10.add_prev_cw(D12);
+    d10.add_prev_ccw(D8);  
+    
+    d11.add_prev_cw(D12);
+    d11.add_prev_ccw(D5);  
+    
+    d12.add_prev_cw(D13);
+    d12.add_prev_ccw(D10); 
+    d12.add_prev_ccw(D11);  
+    
+    d13.add_prev_cw(D0);
+    d13.add_prev_ccw(D12);  
+}
 
 /**
 *
@@ -185,6 +313,37 @@
     return sensor;
 }
 
+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];
+            if(DR_train_pos == prev){
+              found_DR = true;
+              DR_train_pos = prev; //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  
+            }
+     }
+     
+     if(found_DR){
+        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);   
+    }
+    if(!found_DR && !found_LR){
+        lcd.cls();
+        lcd.printf("No train before :(");
+    }
+}
+
 /**
 *
 *Method to catch interrupts 0
@@ -198,8 +357,11 @@
      lcd.cls();
      lcd.printf("int0 0x%x \n Sensor: %d",sensor_data,sensor);
      
+     update_train_pos(sensor);
 }
 
+
+
 /**
 *
 *Method to catch interrupts 1
@@ -212,6 +374,8 @@
      int sensor = get_sensor(sensor_data,1);
      lcd.cls();
      lcd.printf("int1 0x%x \n Sensor: %d",sensor_data,sensor);
+     
+     update_train_pos(sensor);
 }
 
 
@@ -412,6 +576,7 @@
     
     initialize_mcp();   //mcp initialization for interrupts before train running
     init();
+    init_positions();
     
     //Train light routine to start running
     DCC_send_command(DCCaddressDR,DCC_func_lighton,200); // turn light on full