Diff: main.cpp

Revision:

30:63a8a5cefc6b

Parent:

29:6031227dcac9

Child:

31:9d973398554f

--- a/main.cpp	Fri Jun 22 09:55:58 2018 +0000
+++ b/main.cpp	Mon Jun 25 14:57:17 2018 +0000
@@ -6,6 +6,7 @@
 #include "Track.h"
 #include "Detector.h"
 #include <cstdlib>
+#include <algorithm>
 
 //Board 1
 /*----------------------------------------------------------------------------
@@ -19,6 +20,8 @@
 
 DigitalIn sw1(p29), sw2(p30), sw3(p11), sw4(p12);
 
+DigitalOut buzz(p10);
+
 InterruptIn int0(p13), int1(p14);
 I2C i2c(p28, p27);
 MCP23017 *mcp;
@@ -76,6 +79,7 @@
 Function definitions
 *----------------------------------------------------------------------------*/
 bool readSwitch(DigitalIn theSwitch);
+bool sameSection();
 int convertHextoDec(int pos, int intnr);
 void checkDetector(int inter);
 void init_mcp();
@@ -99,6 +103,7 @@
 Main
 *----------------------------------------------------------------------------*/
 int main() {
+    buzz = 0;
     lcd.printf("Start journey");
     init_mcp();
     init_interrupt();
@@ -123,16 +128,35 @@
             executeCase(silverTrain.getPosition(), &silverTrain);
         }
         
+        if (sameSection()){
+            lcd.cls();
+            lcd.printf("SAME SEC");
+            silverTrain.Stop();
+            redTrain.Stop();
+        }     
+        
         if(redTrain.checkStop() && silverTrain.checkStop())  {
             lcd.cls();
             lcd.printf("Stop loop...");
             break;    
-        }      
+        } 
     }
     
     lcd.printf("Shutting down...");
-    // en = 0; TO Do : emergency shutdown!!
-    //wait(3);
+    bool in = true;
+    while(in){ 
+        redTrain.sendCommand();
+        silverTrain.sendCommand();
+        externalLed2 = 1;
+       // wait(0.2);
+        //externalLed2 = 0;
+        buzz = 1;
+        if(!readSwitch(sw3)){
+             in = false;   
+        }
+            
+    }
+    buzz = 0;
     return 0;
 }
 
@@ -191,13 +215,12 @@
         return silverTrain;    
     }
     else{
-    lcd.cls();
-    lcd.printf("NO TRAIN ASSIGNED");
-    lcd.printf("%d", pos);
-    silverTrain.Stop();
-    redTrain.Stop();
-    switch2.switchOff();
-
+        lcd.cls();
+        lcd.printf("NO TRAIN ASSIGNED");
+        lcd.printf("%d", pos);
+        silverTrain.Stop();
+        redTrain.Stop();
+        switch2.switchOff();
     }
 }
 
@@ -224,6 +247,7 @@
        Stop everytime at 2     
        */
        lcd.printf("at 2"); 
+       dect.showReservation();
        if(!dect.checkReservation(10) && !dect.checkReservation(11)){
             currTrain->Stop();
             lcd.printf("Stopping at 2");
@@ -254,10 +278,11 @@
     case 3:{
         lcd.printf("at 3");
        
-        currTrain->changeDirection();               //change direction  
+        
         switch2.switchOff();                        //Disable switch2
         dect.clearReservation(C_left);              //free nr 0,1,2,12,13 
         if(!dect.checkReservation(5)){ 
+            currTrain->changeDirection();               //change direction  
             currTrain->changeSpeed(currTrain->normalSpeed());     //go forward 
         }   
         else{
@@ -282,17 +307,16 @@
     case 5:{
         lcd.printf("at 5");
         
-        currTrain->changeDirection();               //change direction  
         switch3.switchOn();                        //Enable switch3
         dect.clearReservation(C_right);              //free nr 6,7,8
         
-        if(!dect.checkReservation(3)){
-            currTrain->changeSpeed(currTrain->normalSpeed());     //go forward 
+        if(!dect.checkReservation(3)){  
+            currTrain->changeDirection();               //change direction  
+            currTrain->changeSpeed(currTrain->slowlySpeed());     //go forward 
         }   
         else{
             currTrain->Stop();
-        }                                                                
-        
+        }                                                                 
         break;
         }
     case 6:{
@@ -315,7 +339,6 @@
                 dect.makeReservation(junction_511);
                 currTrain->changeSpeed(currTrain->normalSpeed());
             }
-            currTrain->changeSpeed(currTrain->normalSpeed());                          //Go forward
         }         
         break;
         }
@@ -490,6 +513,22 @@
     mcp->_write(GPINTENB, (unsigned char )0xff);// Ready to go!
 }
 
+bool sameSection(){
+    int posRed = redTrain.getPosition();
+    int posSilver = silverTrain.getPosition();
+    
+    if ((std::find(C_left.begin(), C_left.end(), posRed) !=C_left.end()) && (std::find(C_left.begin(), C_left.end(), posSilver) !=C_left.end()))
+        return true;
+    else if ((std::find(C_right.begin(), C_right.end(), posRed) !=C_right.end()) && (std::find(C_right.begin(), C_right.end(), posSilver) !=C_right.end()))
+        return true;
+    else if ((std::find(junction_511.begin(), junction_511.end(), posRed) !=junction_511.end()) && (std::find(junction_511.begin(), junction_511.end(), posSilver) !=junction_511.end()))
+        return true;
+    else if ((std::find(junction_39.begin(), junction_39.end(), posRed) !=junction_39.end()) && (std::find(junction_39.begin(), junction_39.end(), posSilver) !=junction_39.end()))
+        return true;      
+         
+    return false;
+}
+
 void init_trains(){
     redTrain.changeSpeed(stop);
     silverTrain.changeSpeed(stop);