Diff: main.cpp

Revision:

75:bae748a7905f

Parent:

74:dbbc528f2b18

--- a/main.cpp	Wed Jun 27 11:11:28 2018 +0000
+++ b/main.cpp	Wed Jun 27 13:13:07 2018 +0000
@@ -26,8 +26,8 @@
 //INT1 - p10
 InterruptIn int1(p10);
 
-///p11
-///p12
+///p11 - EMPTY
+///p12 - EMPTY
 
 //M0 - p13
 DigitalIn d21stat(p13);     //Sensor right of the station
@@ -36,7 +36,8 @@
 //M2 - p15
 DigitalIn station(p15); //Sensor in the middle of the station
 
-//p16
+//p16 -  EMPTY
+
 //ENABLE - p17
 DigitalOut enable(p17);
 
@@ -52,8 +53,9 @@
 //LCD SCREEN - p21, p22, p23, p24, p25, p26
 TextLCD lcd(p22,p21,p23,p24,p25,p26); // RS, E, A4, A5, A6, A7 // ldc.cls() to clear and printf(String up to 16char)
 
-///p27
-///p28
+//p27 - EMPTY
+
+//12C - p28
 I2C i2c(p28,p27);
 
 //LED1 - p29
@@ -1010,21 +1012,31 @@
     led2 = 0;
     led3 = 0;
         
-    led1 = 1;
     for(int i=0; i<train_back->get_next_sensors().size(); i++){
             
-        led2 = 1;
         for(int j=0; j<train_front->get_previous_sensors().size(); j++){
                 
             while(train_back->get_next_sensors()[i] == train_front->get_previous_sensors()[j]){
                     
+                //lcd.cls();
+                //lcd.printf("TOO CLOSE! S %d", train_back->get_next_sensors()[i]);
+                led1 = 1;
                 led3 = 1;
                 //stop back train
                 train_back->set_speed(STOP);
-            }
+                train_back->run();
+                train_front->run();
+                led1 = 0;
+                led2 = 0;
+                led3 = 0;
+           }
+        led1 = 1;
+        led2 = 1;
+        led3 = 1;
+        train_back->set_speed(MEDIUM);
+        train_back->run();
         }
     }
-    train_back->set_speed(MEDIUM);
     led1 = 0;
     led2 = 0;
     led3 = 0;
@@ -1040,8 +1052,8 @@
 **/
 void check_position(){    
     
-    stay_at_distance(&DR_train,&LR_train);
-    stay_at_distance(&LR_train,&DR_train);
+    //stay_at_distance(&DR_train,&LR_train);
+    //stay_at_distance(&LR_train,&DR_train);
     
     if(check_NAC()){ NAC_action(); }
     
@@ -1420,13 +1432,13 @@
     initialize_mcp();   //mcp initialization for interrupts before train running
     init();
        
-    int DR_init_sensor = select_sensor(D6,"DR");
-    bool DR_init_dir = select_direction(true,"DR");
+    int DR_init_sensor = select_sensor(D4,"DR");
+    bool DR_init_dir = select_direction(false,"DR");
     
     wait(0.5);
     
-    int LR_init_sensor = select_sensor(D3,"LR");
-    bool LR_init_dir = select_direction(true,"LR");
+    int LR_init_sensor = select_sensor(D10,"LR");
+    bool LR_init_dir = select_direction(false,"LR");
     
     DR_train.set_position(DR_init_sensor);
     DR_train.set_goes_cw(DR_init_dir);
@@ -1465,12 +1477,13 @@
     
     //Demo for stopping at the station
     while(1) {
-        
+    
+        DR_train.run();
+        LR_train.run(); 
+        check_position();   
+        //checkSwitch();  //Checks for switch commands everytime. 
+        //switch_toSpeed();
 
-        checkSwitch();  //Checks for switch commands everytime. 
-        switch_toSpeed();
-        lcd.cls();
-        check_position();   
     }
 }