Diff: main.cpp

Revision:

62:6fc0c683cb02

Parent:

59:032005c5a495

Child:

63:86f8dfa007ac

--- a/main.cpp	Thu Jun 21 17:55:51 2018 +0000
+++ b/main.cpp	Mon Jun 25 10:43:54 2018 +0000
@@ -4,9 +4,13 @@
 #include <string>
 #include <iostream>
 #include <vector>
+#include <fstream>
+#include <sstream>
 
 using namespace std;
 
+
+LocalFileSystem local("local");
  
 /******PINS AND DECLARATIONS*******/
 
@@ -35,7 +39,6 @@
 DigitalIn station(p15); //Sensor in the middle of the station
 
 //p16
-
 //ENABLE - p17
 DigitalOut enable(p17);
 
@@ -52,8 +55,7 @@
 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
-
-//I2C - p28
+///p28
 I2C i2c(p28,p27);
 
 //LED1 - p29
@@ -92,7 +94,7 @@
 #define D21 14
 #define D22 15
 
-//Definition of the speeds trains can drive at. Will be interpreted as ints for the program's logic
+
 #define STOP 0
 #define SLOW 1
 #define MEDIUM 2
@@ -101,6 +103,7 @@
 #define R_MEDIUM 5
 
 
+
 /**
 *
 *Position class.
@@ -113,11 +116,9 @@
 *
 *get_pos() - 
 *get_prev_cw() - 
-*get_next_cw() - 
-*add_prev_ccw() - 
-*add_cnext_ccw() - 
-*add_prev_ccw() -
+*get_ccw() - 
 *add_prev_cw() - 
+*add_ccw() - 
 *
 **/
 class Position{
@@ -125,7 +126,6 @@
         int position; 
         vector <int> previous_cw;
         vector <int> previous_ccw;
-        
     public:
         Position(int p){
             position = p;
@@ -220,7 +220,6 @@
     }
 }
 
-
 /**
 *Defining areas for train detection and collision logic.
 *area_A_arr/area_B_arr - Arrays that hold the Dsensors for each area, used to initialize the vectors.
@@ -233,35 +232,29 @@
 const vector<int> area_B(area_B_arr,area_B_arr + sizeof(area_B_arr) / sizeof(int));
 
 
+
 /**
 *
 *Train class.
 *
-*@train_address - 
 *@position - 
 *@going_cw - 
-*@speed - 
 *
 *Train(int, bool) - 
-*Train(address, s) - 
+*Train(bool) - 
 *
-*set_speed(int) - 
+*Vector get_next_sensors() - 
 *set_position(int) - 
 *set_goes_cw(bool) - 
-*
-*Vector get_next_sensors() - 
 *Position get_position() - 
 *Int get_position_number() - 
 *Bool goes_cw() -
-*Bool is_in_A() - 
-*Bool is_in_B() - 
-*
-*run(int) - 
 *
 **/
 class Train{
     
-    private:               
+    private:       
+        
         unsigned int train_address;    //stop the train        
         Position *position;
         bool going_cw;
@@ -275,19 +268,25 @@
         }
         
         /**
-        *Constructor that takes the address of the train and the speed with default value MEDIUM.
-        **/
-        Train(unsigned int address, int s = MEDIUM){
-            
+        * Contructor that takes the address of the train and the speed with default value MEDIUM.
+        */
+        Train(unsigned int address, int s=MEDIUM){
             train_address = address;
             speed = s;
         }
-                                
+        
+        Train(bool cw){ going_cw = cw; }
+                        
         vector<int> get_next_sensors(){
             
             //Checking direction
-            if(going_cw){return position->get_next_cw();}
-                else{return position->get_next_ccw();}            
+            if(going_cw){
+                
+                return position->get_next_cw();
+            }else{
+                
+                return position->get_next_ccw();
+            }            
         }
         
         void set_speed(int s){
@@ -298,7 +297,7 @@
         *   Sends a DCC command to the train with the speed indicaed by the attribute speed
         *   The number of times the command is sent can be indicated as an optional parameter. Default value is 1.
         */        
-        void run(int times = 1){
+        void run(int times=1){
             
             const unsigned int DCCinst_forward_slow = 0x66; //forward slow speed (step 9)
             const unsigned int DCCinst_forward_medium = 0x68; //forward half speed
@@ -308,27 +307,21 @@
             const unsigned int DCCinst_stop = 0x50;    //stop the train
             
             switch(speed){
-                
-                case STOP:                
+                case STOP:
                     DCC_send_command(train_address, DCCinst_stop,times);
                     break;
-                    
                 case SLOW:
                     DCC_send_command(train_address, DCCinst_forward_slow,times);
                     break;
-                    
                 case MEDIUM:
                     DCC_send_command(train_address, DCCinst_forward_medium,times);
                     break;
-                    
                 case FAST:
                     DCC_send_command(train_address, DCCinst_forward_fast,times);
                     break;
-                    
                 case FULL:
                     DCC_send_command(train_address, DCCinst_forward_full,times);
                     break;
-                    
                 case R_MEDIUM:
                     DCC_send_command(train_address, DCCinst_reverse_medium,times);
                     break;
@@ -360,6 +353,7 @@
             return going_cw;
         }
         
+        
         /**
         *
         *Checks if the element exists within the vector.
@@ -383,13 +377,13 @@
         }
         
         bool is_in_A(){
+            return in_vector(area_A,get_position_number());
             
-            return in_vector(area_A, get_position_number());
         }
         
         bool is_in_B(){
             
-            return in_vector(area_B, get_position_number());
+            return in_vector(area_B,get_position_number());
         }
 };
 
@@ -453,19 +447,131 @@
 *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(DCCaddressDR,MEDIUM);
+Train DR_train(DCCaddressDR,FAST);
 Train LR_train(DCCaddressLR,MEDIUM);
 
 //possibility of an array having {dr_train, lr_train}? for reuse and modularity of functions
 
 
+//****** SYSTEM LOGGER FUNCTIONS ******//
+
 /**
-*Booleans that will determine if the train should be moving or not. 
-*Booleans will switch to false to stop any of the trains and avoid collisions.
-*DR_run - Boolean for DR_train / LR_run - Boolean for LR_train
+*
+*
+*
+**/
+string train_dir_toString(Train* train){
+ 
+    string dir_string;
+    
+    if(train->goes_cw()){
+        
+        dir_string = "cw";
+    }else{
+        
+        dir_string = "ccw";
+    }
+    return dir_string;
+}
+
+/**
+*
+*
+*
+**/
+string train_pos_toString(Train* train){
+    
+    int i = train->get_position_number();
+    std::string tr_pos;
+    std::stringstream ss;
+    
+    ss << i;    
+    tr_pos = ss.str();
+    
+    return tr_pos;
+}
+
+
+/**
+*
+*
+*
+**/
+void logger_add(string s){
+    
+    FILE *fp = fopen("/local/sys_log.txt", "a"); 
+    fprintf(fp, "%s \n", s); 
+    fclose(fp);
+    
+}
+
+
+/**
+*
+*
+*
 **/
-bool DR_run = true;
-bool LR_run = true;
+void init_logger(){
+    
+    FILE *fp = fopen("/local/sys_log.txt", "w"); 
+    
+    fprintf(fp, "Log for the main train program: \n"); 
+    fprintf(fp, "------------------------------- \n \n"); 
+    
+    fclose(fp);
+
+}
+
+/**
+*
+*
+*
+**/
+void train_pos0_log(){
+    
+    FILE *fp = fopen("/local/sys_log.txt", "a"); 
+    
+    fprintf(fp, "DR_train START: D%s sensor, %s direction. \n", train_pos_toString(&DR_train), train_dir_toString(&DR_train)); 
+    fprintf(fp, "LR_train START: D%s sensor, %s direction. \n \n", train_pos_toString(&LR_train), train_dir_toString(&LR_train)); 
+    
+    fprintf(fp, "..............Program Start................ \n");
+        
+    fclose(fp);
+
+}
+
+void print_next_sensors(Train* train){
+     
+    FILE *fp = fopen("/local/sys_log.txt", "a"); 
+
+    for(int i=0; i<train->get_next_sensors().size(); i++){ 
+           
+        fprintf(fp, "%d" ,train->get_next_sensors()[i]);
+    }
+    
+    fprintf(fp, "\n");
+    fclose(fp);  
+
+}
+
+void trains_posn_log(int sensor){
+ 
+    FILE *fp = fopen("/local/sys_log.txt", "a"); 
+    
+    fprintf(fp, "SIGNAL FROM SENSOR D%d \n", sensor);
+    fprintf(fp, "DR_train: D%s sensor, %s direction. // Next positions:  ", train_pos_toString(&DR_train), train_dir_toString(&DR_train)); 
+    print_next_sensors(&DR_train);
+       
+    fprintf(fp, "LR_train: D%s sensor, %s direction. // Next positions: ", train_pos_toString(&LR_train), train_dir_toString(&LR_train)); 
+    print_next_sensors(&LR_train);
+    fprintf(fp, "\n");
+
+    fclose(fp);  
+}
+
+
+
+
 
 
 
@@ -618,6 +724,63 @@
     return sensor;
 }
 
+/**
+*
+*Method to flip the switches
+*
+*@switchId - (1-4)The ID of the switch we want to flip
+*@times - The number of times we want to send the command
+*@activate - True if the switch is going to be activated. False if it needs to go back to rest position.
+*
+**/
+void flip_switch(int switchId, int times, bool activate=true){
+    
+    unsigned int SWBflip = SWBidle; //IDLE - Flip last activated SW.
+    
+    switch(switchId){
+        
+        case 1:
+            SWBflip = SWBflip_1;     //FLIP SW1
+            break;
+            
+        case 2:
+            SWBflip = SWBflip_2;     //FLIP SW2
+            break;
+            
+        case 3:
+            SWBflip = SWBflip_3;     //FLIP SW3
+            break;
+            
+        case 4:
+            SWBflip = SWBflip_4;     //FLIP SW4
+            break;
+            
+        default:
+            break;    
+    }          
+
+    //Security measure not to burn the switch.
+    if(times <=5){ 
+    
+        DCC_send_command(SWBaddress,SWBflip,times); //Activating switch
+        if(!activate){
+              
+              DCC_send_command(SWBaddress,SWBidle,times);  //Sending IDLE to flip back.
+        }
+     }
+}
+
+/**
+* Action to do when NAC is detected 
+* Booster is disabled and the buzz is activated
+* 
+*/
+void NAC_action(){
+    enable = 0;
+    doBuzz();
+}
+
+
 
 /**
 *This method will check if there is a non-avoidable frontal collision(NAFC).
@@ -653,21 +816,34 @@
     return NAC;     
 }
 
-
-/*
-void AFC_action(int switch_n, int sensor, Train *stop_train, Train * cont_train ){
+/**
+* Switch_n switch that needs to switch
+´* cont_sensor sensor that when activated the stopped train continues
+* switch_sensor sensor where the switch should be activated
+*/
+void AFC_action(int switch_n, int cont_sensor, int switch_sensor, Train *stop_train, Train * cont_train ){
+    bool send_pack_switch = false;
     
-    flip_switch(switch_n,5); //Activate switch 5
+    //flip_switch(switch_n,5);
     
-    while(cont_train->get_position_number() != sensor){
-        DR_run = false; //DR has to stop
-        LR_run = true; //LR continues
-        send_command();
+    while(cont_train->get_position_number() != cont_sensor){
+        if(cont_train->get_position_number() == switch_sensor){
+            send_pack_switch = true;          
+        }
+        stop_train->set_speed(STOP);
+        stop_train->run(); //Stopping train on sensor D4 or D10
+        cont_train->run();
+        
+        if(send_pack_switch){
+            lcd.cls();
+            lcd.printf("Switching SW%d",switch_n);
+            flip_switch(switch_n,5);
+        }
     }
-    DR_run = true;
-    LR_run = true; //Both trains start to run again
+    
+    flip_switch(5,5); //Send IDLE command
+    
 }
-*/
 
 
 
@@ -679,51 +855,73 @@
 *Train in area A(ccw) and train in D4(cw)
 *Train in area A(cw) and train in D10(ccw)
 *Train in area B(cw) and train in D4(ccw)
-*Train in area B(ccw) and train in D10(ccw) 
+*Train in area B(ccw) and train in D10(ccw)
+*
+*stop_train is the train that is going to stop in sensors D4 or D10 until the other train passes
+*cont_train is the train that won't stop and will do the switch 
 *
 **/
-void check_AFC(bool DR_in_A, bool DR_in_B,bool LR_in_A,bool LR_in_B){   //TODO - Add same for LR train
-    
-    if( DR_train.get_position_number() == D4){  
+bool check_AFC(Train *stop_train, Train *cont_train){   //TODO - Add same for LR train
+    bool detected_AFC = false;
+    if( stop_train->get_position_number() == D4){  
              
-            if(DR_train.goes_cw()){
+            if(stop_train->goes_cw()){
                 
-                if(LR_in_A && !LR_train.goes_cw()){
+                if(cont_train->is_in_A() && !cont_train->goes_cw()){
+                    
+                    lcd.cls();
+                    lcd.printf("AFC!!! STOP D4 SW2 CONT D3");
                     
-                //Activate switch2
-                //DR_train has to stop
-                //When LR is at D3 DR continues
+                    AFC_action(2,D3,D2,stop_train,cont_train); 
+                    //Activate switch2
+                    //When cont_train is at D3 stop_train continues
+                
+                detected_AFC = true;
                 }
             }else{ //DR goes ccw
             
-                if(LR_in_B && LR_train.goes_cw()){
+                if(cont_train->is_in_B() && cont_train->goes_cw()){
                     
+                    lcd.cls();
+                    lcd.printf("AFC!!! STOP D4 SW3 CONT D5");
+                    
+                    AFC_action(3,D5,D6,stop_train,cont_train); 
                     //DR_train stops
                     //Activate switch3
                     //When LR is at D5 DR continues
+                    detected_AFC = true;
                 }
             }
             
-    }else if(DR_train.get_position_number() == D10){
+    }else if(stop_train->get_position_number() == D10){
         
-        if(DR_train.goes_cw()){
+        if(stop_train->goes_cw()){
             
-            if(LR_in_B && !LR_train.goes_cw()){
+            if(cont_train->is_in_B() && !cont_train->goes_cw()){
+                lcd.cls();
+                lcd.printf("AFC!!! STOP D10 SW4 CONT D9");
                 
+                AFC_action(4,D9,D8,stop_train,cont_train); 
                 //DR train stops
                 //Activate switch4
                 //When LR is at D9 DR continues
+                detected_AFC = true;
             }
         }else{
             
-            if(LR_in_A && LR_train.goes_cw()){
-                
+            if(cont_train->is_in_A() && cont_train->goes_cw()){
+                lcd.cls();
+                lcd.printf("AFC!!! STOP D10 SW1 CONT D11");
+                AFC_action(1,D11,D12,stop_train,cont_train); 
                 //DR train stops
                 //Activate switch1
-                //When LR is at D9 DR continues
+                //When LR is at D11 DR continues
+                detected_AFC = true;
             }
         }
     }
+    
+    return detected_AFC;
 }
 
 
@@ -738,7 +936,12 @@
     if(check_NAC()){
         lcd.cls();
         lcd.printf("NAC!!!");
-    }      
+        NAC_action();
+    }
+    check_AFC(&DR_train,&LR_train);       
+        
+    check_AFC(&LR_train,&DR_train);
+            
     
 }
 
@@ -751,31 +954,33 @@
 *
 **/
 void update_train_pos(int sensor){
+    led2 = 1;
     
     bool found_DR = false;
     bool found_LR = false;
-    string DR_dir, LR_dir;
+    
+    string DR_dir,LR_dir;
     
     if(DR_train.goes_cw()){
-        
         DR_dir = "cw";
     }else{
-        
         DR_dir = "ccw";
     }
     
     if(LR_train.goes_cw()){
-        
         LR_dir = "cw";
     }else{
-        
         LR_dir = "ccw";
     }    
-        
+    
+    
+    //wait(0.7);   
+    
     if(sensor == DR_train.get_position_number() || sensor == LR_train.get_position_number()){
-        //Ignore
+        led2 = 0;        
+    }else{
         
-    }else{
+        //trains_posn_log(sensor);
         
         lcd.cls();
         lcd.printf("S:D%d DR%d(",sensor,DR_train.get_position_number());
@@ -794,7 +999,8 @@
         }
         
         lcd.printf(")%s",LR_dir);
-    
+        
+
         //Checking next sensors for DR train
         for(int i=0; i<DR_train.get_next_sensors().size(); i++){         
             
@@ -864,7 +1070,9 @@
             lcd.printf("No train before :(");
         }
         */
+        
     }
+    
 }
 
 
@@ -901,13 +1109,12 @@
      update_train_pos(sensor);
 }
 
-
 /**
 *
 *Clear current interrupts 
 *
 **/
-void init(){ 
+void init() { 
 
     mcp->_read(GPIOA); 
     mcp->_read(GPIOB); // Register callbacks 
@@ -915,54 +1122,13 @@
     int1.fall(&on_int1_change); // Enable interrupts on MCP 
     mcp->_write(GPINTENA, (unsigned char )0xff); 
     mcp->_write(GPINTENB, (unsigned char )0xff); // Ready to go! 
-}
+  }
+
 
 
-/**
-*
-*Method to flip the switches
-*
-*@switchId - (1-4)The ID of the switch we want to flip
-*@times - The number of times we want to send the command
-*@activate - True if the switch is going to be activated. False if it needs to go back to rest position.
-*
-**/
-void flip_switch(int switchId, int times, bool activate=true){
-    
-    unsigned int SWBflip = SWBidle; //IDLE - Flip last activated SW.
-    
-    switch(switchId){
-        
-        case 1:
-            SWBflip = SWBflip_1;     //FLIP SW1
-            break;
-            
-        case 2:
-            SWBflip = SWBflip_2;     //FLIP SW2
-            break;
-            
-        case 3:
-            SWBflip = SWBflip_3;     //FLIP SW3
-            break;
-            
-        case 4:
-            SWBflip = SWBflip_4;     //FLIP SW4
-            break;
-            
-        default:
-            break;    
-    }          
+
 
-    //Security measure not to burn the switch.
-    if(times <=5){ 
-    
-        DCC_send_command(SWBaddress,SWBflip,times); //Activating switch
-        if(!activate){
-              
-              DCC_send_command(SWBaddress,SWBidle,times);  //Sending IDLE to flip back.
-        }
-     }
-}
+
 
 
 /**
@@ -1015,33 +1181,29 @@
     bool exit = false;
     
     while(!exit){
-        
         if(switch3 == 0){
-            
             if(changed){
-               
                sensor++;
                sensor=sensor%15; //Only sensors from 0 to 15.
                changed=false; 
                lcd.cls();
                lcd.printf("%s: D%d",train,sensor);
             }
+            
         }else{
-            
             changed = true;
             wait(0.2);
         }
         
         if(switch4 == 0){
-            
             exit = true;
             wait(0.2);
         }
     }
+    
     return sensor;
 }
 
-
 /**
 * Returns a boolean representing the direction. Everytimew switch3 is 0 it changes the direction.
 * When switch4 is 0 the selection is confirmed.
@@ -1053,10 +1215,8 @@
     string dir_string;
     
     if(init_going_cw){
-        
         dir_string = "cw";
     }else{
-        
         dir_string = "ccw";
     }
     
@@ -1068,76 +1228,33 @@
     bool changed = false;
     
     while(!exit){
-        
         if(switch3 == 0){
-            
             if(changed){
-                
                 going_cw = !going_cw;
                 changed = false;
                 string dir;
-                
                 if(going_cw){
-                    
                     dir = "cw";
                 }else{
-                    
                     dir = "ccw";
                 }
-        
                 lcd.cls();
                 lcd.printf("%s: %s",train,dir);
             }
         }else{
-            
             changed = true;
             wait(0.2);
         }
         
         if(switch4 == 0){
-            
             exit = true;
             wait(0.2);
         }
     }
+    
     return going_cw;
 }
 
-
-/**
-*
-*
-*
-**/
-void adjustSpeed(){
-    
-    float f = pot.read();
-    float vin = f * 3.3;
-    lcd.cls();
-   // lcd.printf("vin: %.4f",vin);
-    
-    if(0=< vin && vin< 0.60){
-        
-        //speed = slow
-        lcd.printf("SLOW AF");       
-        }else if(0.60 < vin && vin< 1.20){
-            
-            //speed medium
-            lcd.printf("MEDIUM");
-            }else if(1.20 < vin && vin< 2.20){
-                
-                //speed fast
-                lcd.printf("going fast boii");
-                }else if(2.20 < vin && vin<3.20){
-                    
-                    //speed full
-                    lcd.printf("full POWAH BABY");
-                    }
-}
-
-
-
-
 //**************** MAIN PROGRAM FOR DENVER TRAIN ****************//
 
 
@@ -1219,8 +1336,22 @@
     lcd.printf("Ready to start");
     wait(1);
     
+    //init_logger();
+    //train_pos0_log();
+    //logger_add("There we go lads this is the start of the program");
+    
     enable = 1;
     
+    flip_switch(5,5);//Send IDLE command at the beginning
+    flip_switch(1,5);
+    flip_switch(5,5);
+    flip_switch(2,5);
+    flip_switch(5,5);
+    flip_switch(3,5);
+    flip_switch(5,5);
+    flip_switch(4,5);
+    flip_switch(5,5);
+    
     //Demo for stopping at the station
     while(1) {