eb_test: converting latch to fsm

[eurobot/public.git] / src / eb_test / main.c

diff --git a/src/eb_test/main.c b/src/eb_test/main.c
index 762809379b6de798b347a6ec3111c5089ef2a9cc..6fff96afc175956d0b214e25b46ce6593548ef0b 100644 (file)
--- a/src/eb_test/main.c
+++ b/src/eb_test/main.c
@@ -331,7 +331,7 @@ int main(void)
        uint32_t status_time = timer_usec;
        can_msg_t tx_msg;
        bool obrat=true;
-         bool button_status[3]; //not using button_status[0], begging from 1 and giving the value of pin 1 is making sens
+         bool button_status[3]; 
        int i=0,delta=0,j=0,suma=0;
        
 
@@ -353,7 +353,7 @@ int main(void)
        SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0);
        SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0); 
 
-//     IO0DIR &= ~             
+       
        IO0DIR &= ~((1<<START_PIN) | (1<<BUMPER_RIGHT) | (1 << COLOR_PIN));
        
        IO0DIR &= ~((1<<BUMPER_PIN) | (1<<BUMPER_LEFT));  //first shift one on the right pin
@@ -361,14 +361,18 @@ int main(void)
        
        
        
-       //IO1DIR &= ~(3<<20);
+
 
        send_rs_str("Vidle started\n"); 
        
        // The above send_rs_str is importat - we wait for the first AD conversion to be finished
        fsm_vidle.act_pos = adc_val[0];
-       
-
+         button_status[1] = (IO0PIN & (1<<BUTTON_1_PIN)) == 0;  //reading value (inspiration from START_PIN)
+         button_status[2] = (IO0PIN & (1<<BUTTON_2_PIN)) == 0; 
+         fsm_vidle.button1=button_status[1];
+         fsm_vidle.button2=button_status[2];
+       init_fsm(&fsm_vidle, &fsm_vidle_init);
+       engine_A_dir(ENGINE_DIR_FW);
 /*     test_vhn(); */
 /*     return; */      
        
@@ -382,11 +386,19 @@ int main(void)
                        main_time = timer_usec;
 
                        //dbg_print_time();
-
+                       fsm_vidle.last_pos = fsm_vidle.act_pos;
                        fsm_vidle.act_pos = adc_val[0];
+                       fsm_vidle.delta=fsm_vidle.act_pos-fsm_vidle.last_pos;
+                       button_status[1] = (IO0PIN & (1<<BUTTON_1_PIN)) == 0;  //reading value (inspiration from START_PIN)
+                       button_status[2] = (IO0PIN & (1<<BUTTON_2_PIN)) == 0; 
+               //        if (button_status[1]) { send_rs_str("pozadavek na  uzavreni zavory"); send_rs_str("\n");} 
+               //            else { send_rs_str("pozadavek na  otevreni zavory"); send_rs_str("\n");}
+                 //      if (button_status[2]) { send_rs_str("pozadavek na ignoraci prekazek"); send_rs_str("\n");}                                            
+                       //      else { send_rs_str("pozadavek na uhybani prekazkam"); send_rs_str("\n");}
+                       fsm_vidle.button1=button_status[1];
+                       fsm_vidle.button2=button_status[2]; 
+                       run_fsm(&fsm_vidle);
                        
-                   
-                       //      run_fsm(&fsm_vidle);
                }
 
                if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
@@ -400,48 +412,8 @@ int main(void)
                //start_button();
                //handle_bumper();
 
-                 button_status[1] = (IO0PIN & (1<<BUTTON_1_PIN)) == 0;  //reading value (inspiration from START_PIN)
-                 button_status[2] = (IO0PIN & (1<<BUTTON_2_PIN)) == 0; 
-                 if (button_status[1]) { send_rs_str("nekdo zmacknul cudlik 1"); send_rs_str("\n");}
                  
-                 if (button_status[2]) { send_rs_str("nekdo zmacknul cudlik 2"); send_rs_str("\n");}
-                 dummy_wait();
-                 
-       /*      delta=i;
-               i=adc_val[0];
-               delta=i-delta;
-               suma+=delta;    
-               engine_A_pwm(20);
-               if (j++>10000) {
-                 if (abs(suma)<1) {obrat=!obrat;}
-               send_rs_int(suma);
-               send_rs_str("\n");
-                 j=0;
-                 suma=0;
-                 
-               }
-               if (obrat)
-                 {
-                   if (i>300) 
-                   engine_A_dir(ENGINE_DIR_BW);
-                   else {engine_A_dir(ENGINE_DIR_FW); 
-                   obrat=false;
-                   suma=20;  
-                   }
-                 }
-                  else
-                  {
-                    if(i<900)
-                      engine_A_dir(ENGINE_DIR_FW);
-                   else {engine_A_dir(ENGINE_DIR_BW); 
-                   obrat=true;
-                   suma=20;  
-                   }
-                    
-                  }
        
-               
-       */