Diff: main.cpp

Revision:

52:c08495446f87

Parent:

51:badef9fc202f

Child:

53:1ffd622bab24

Child:

55:aa3baa01f43d

--- a/main.cpp	Wed Jun 20 10:08:20 2018 +0000
+++ b/main.cpp	Wed Jun 20 12:54:28 2018 +0000
@@ -295,7 +295,7 @@
 *Using boolean constructor because position initialization will be done after initializing all position vectors.
 *DR_train = Dark Red train - LR_train = Light Red Train
 **/
-Train DR_train(true); //Position and going_cw
+Train DR_train(false); //Position and going_cw
 Train LR_train(true);
 
 //possibility of an array having {dr_train, lr_train}? for reuse and modularity of functions
@@ -601,6 +601,7 @@
 *
 **/
 void update_train_pos(int sensor){
+    led2 = 1;
     
     bool found_DR = false;
     bool found_LR = false;
@@ -621,7 +622,7 @@
     
     
     lcd.cls();
-    lcd.printf("S:D%d DR(",sensor);
+    lcd.printf("S:D%d DR%d(",sensor,DR_train.get_position_number());
     
     //TODO: Do a for to print all next sensors.
     for(int i=0; i<DR_train.get_next_sensors().size(); i++){ 
@@ -629,7 +630,7 @@
         lcd.printf("%d,",DR_train.get_next_sensors()[i]);
     }
     
-    lcd.printf(")%s LR(",DR_dir);
+    lcd.printf(")%s LR%d(",DR_dir,LR_train.get_position_number());
     
     for(int i=0; i<LR_train.get_next_sensors().size(); i++){ 
        
@@ -637,9 +638,14 @@
     }
     
     lcd.printf(")%s",LR_dir);
-    wait(0.7);   
+    //wait(0.7);   
     
-    //Checking next sensors for DR train
+    if(sensor == DR_train.get_position_number()){
+        led2 = 0;
+        //lcd.cls();
+        //lcd.printf("Sensor TWICE!!");
+    }else{
+        //Checking next sensors for DR train
     for(int i=0; i<DR_train.get_next_sensors().size(); i++){         
         
         if(DR_train.get_next_sensors()[i] == sensor){ //If the sensor is one expected to visit by the train we update the position
@@ -660,7 +666,8 @@
                 }
             } 
             
-            DR_train.set_position(sensor);               
+            DR_train.set_position(sensor);
+                           
         }
     }
     
@@ -683,10 +690,15 @@
                     LR_train.set_goes_cw(true); //If train goes ccw and passes D9 or D3 we change orientation
                 }
             }
-            LR_train.set_position(sensor);  
+            LR_train.set_position(sensor);
+             
         }
     }
+    }
+    
+    
                    
+     /*
      if(found_DR){
         
         //doBuzz();
@@ -705,6 +717,7 @@
         lcd.cls();
         lcd.printf("No train before :(");
     }
+    */
 }
 
 
@@ -909,14 +922,37 @@
     
     if(LR_run){
         
-        DCC_send_command(DCCaddressLR,DCCinst_forward,1); // Forward half speed train addres DARK-RED 
+        DCC_send_command(DCCaddressLR,DCCinst_forward_fast,1); // Forward half speed train addres DARK-RED 
     }else{
         
         DCC_send_command(DCCaddressLR,DCCinst_stop,400);
     }
 }
 
-
+int select_sensor(){
+    int sensor = 0;
+    bool changed = false;
+    bool exit = false;
+    
+    while(!exit){
+        if(switch3 == 0){
+            if(changed){
+               sensor++;
+               sensor=sensor%15; //Only sensors from 0 to 15.
+               changed=false; 
+            }
+            
+        }else{
+            changed = true;
+        }
+        
+        if(switch4 == 0){
+            exit = true;
+        }
+    }
+    
+    return sensor;
+}
 
 //**************** MAIN PROGRAM FOR DENVER TRAIN ****************//
 
@@ -947,14 +983,14 @@
     init();
     init_positions();
     
-    DR_train.set_position(D4);
-    LR_train.set_position(D10);
+    DR_train.set_position(D10);
+    LR_train.set_position(D4);
     
     //Train light routine to start running
     /*
     DCC_send_command(DCCaddressDR,DCC_func_lighton,200); // turn light on full
-    DCC_send_command(DCCaddressDR,DCC_func_dimlight,400); //dim light
-    DCC_send_command(DCCaddressDR,DCC_func_lighton,200);  //light full again
+    DCC_send_command(DCCaddressDR,DCC_func_dimlight,400); // dim light
+    DCC_send_command(DCCaddressDR,DCC_func_lighton,200);  // light full again
     */
 
     //LED3 Shows start of route + LCD notif