vidle: clean up

diff --git a/src/eb_vidle/main.c b/src/eb_vidle/main.c
index 340983a9c4da31bd82dc4a0658ab98f2cf8c5345..80b129fcea49059a4b3dd386ac9585367eea416a 100644 (file)
--- a/src/eb_vidle/main.c
+++ b/src/eb_vidle/main.c
@@ -124,6 +124,12 @@ void start_button(void)
        
        start_condition = (IO0PIN & (1<<START_PIN)) == 0;
 
+       if (start_condition)
+               deb_led_on(LEDY);
+       else
+               deb_led_off(LEDY);
+
+
        if (start_condition != last_start_condition) {
                last_start_condition = start_condition;
                count = 0;
@@ -131,7 +137,6 @@ void start_button(void)
        }
 
        if (timer_msec >= next_send) {
-               deb_led_on(LEDY);
                msg.id = CAN_ROBOT_CMD;
                msg.flags = 0;
                msg.dlc = 1;
@@ -227,19 +232,66 @@ void dbg_print_time()
 void fsm_vidle_init(struct fsm *fsm, enum event event);
 
 
+void blink_led()
+{
+       static uint32_t led_time = 0;
+       
+       if(timer_msec >= led_time + 500)        
+       {
+               led_time = timer_msec;
+               /*  static int up;
+                   if (up == 0)
+                   fsm_vidle.can_req_position = 0x380;
+                   if (up == 6)
+                   fsm_vidle.can_req_position = 0x1e0;
+                   up = (up+1)%12;
+               */
+               deb_led_change(LEDG);
+               send_rs_int(fsm_vidle.act_pos);
+               send_rs_str("\n");
+       }
+}
 
 
 #define BUMPER_PIN     17              // bumper pin  (SCK1)
 
+void handle_bumper()
+{
+       static uint32_t bumper_time = 0;
+
+       if (timer_msec >= bumper_time + 100)    
+       {
+               can_msg_t tx_msg;
+
+               bumper_time = timer_msec;
+               
+               tx_msg.id = CAN_BUMPER;
+               tx_msg.dlc = 1;
+               tx_msg.flags = 0;       
+                       
+                       
+               if (IO0PIN & (1<<BUMPER_PIN))
+               {    
+                       tx_msg.data[0] = 1;
+                       deb_led_on(LEDR);
+               }
+               else
+               {
+                       tx_msg.data[0] = 0;
+                       deb_led_off(LEDR);
+               }
+                       
+               can_tx_msg(&tx_msg);
+       }
+}
+
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 int main(void)
 {
        uint32_t main_time = timer_usec;
-       uint32_t led_time = timer_msec;
-       uint32_t bumper_time = timer_msec;
-       uint32_t CAN_time = timer_msec;
+       uint32_t status_time = timer_usec;
        
 
        init_periphery();
@@ -266,8 +318,6 @@ int main(void)
        fsm_vidle.can_req_position = adc_val[0];
        
        while(1){
-         
-               start_button();
                if(timer_usec >= main_time + 1000)
                {
                        main_time = timer_usec;
@@ -280,54 +330,18 @@ int main(void)
                        run_fsm(&fsm_vidle);
                }
 
-#if 1
-               if (timer_msec >= CAN_time + 100 || //repeat sending message every 100 ms
+               if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
                    fsm_vidle.trigger_can_send) {   //or when something important happen
                        fsm_vidle.trigger_can_send = false;
-                       CAN_time = timer_msec; //save new time, when message was sent
+                       status_time = timer_msec; //save new time, when message was sent
                        can_send_status();
 
                }
-#endif 
-               if(timer_msec >= led_time + 500)        
-               {
-                       /*  static int up;
-                           if (up == 0)
-                           fsm_vidle.can_req_position = 0x380;
-                           if (up == 6)
-                           fsm_vidle.can_req_position = 0x1e0;
-                           up = (up+1)%12;
-                       */      led_time = timer_msec;
-                       deb_led_change(LEDG);
-                       send_rs_int(fsm_vidle.act_pos);
-                       send_rs_str("\n");
-               }
-               
-               if(timer_msec >= bumper_time + 100)     
-               {
-                 
-                 
-                       can_msg_t tx_msg;
-                       tx_msg.id = CAN_BUMPER;
-                       tx_msg.dlc = 1;
-                       tx_msg.flags = 0;       
-                       
-                       
-                       if (IO0PIN & (1<<BUMPER_PIN))
-                       {    
-                               tx_msg.data[0] = 1;
-                               deb_led_on(LEDB);
-                       }
-                       else
-                       {
-                               tx_msg.data[0] = 0;
-                               deb_led_off(LEDB);
-                       }
-                       
-                       can_tx_msg(&tx_msg); /* CAN erratum workaround */
-               }
-               
-               
+
+               start_button();
+               handle_bumper();
+
+               blink_led();
        }
 }