eb_lift: Send real position of the lift after it reaches its target state

diff --git a/src/eb_lift_09/def.h b/src/eb_lift_09/def.h
index 0cdf5fcbcc9145a44b138be3fb7c05bd247d7440..076ffc0262110fa1f2dca816bc948f9f9a4d226b 100644 (file)
--- a/src/eb_lift_09/def.h
+++ b/src/eb_lift_09/def.h
@@ -102,6 +102,8 @@
 #define LIFT_PUSHER_ERR_POS    0x20        //< Pusher position cannot be reached 
 
        volatile uint8_t can_flags;         //< can  flags bit
-       bool can_send_now;
+
+       void can_send_status(void);
+
 
 #endif

diff --git a/src/eb_lift_09/lift.c b/src/eb_lift_09/lift.c
index f1106b012bd4291cc7c7499ef13f3128fd9d7d6b..5ec768350ea85be06d2731999e9cf6ab4f2269a8 100644 (file)
--- a/src/eb_lift_09/lift.c
+++ b/src/eb_lift_09/lift.c
@@ -213,7 +213,7 @@ void fsm_homing_bw(struct fsm *fsm, events my_event){
                          fsm->act_pos = 0;                             // reset position
                          fsm->current_state = &fsm_homing_fw_up;               // move to fsm_stop
                          fsm->ans_can = 0;
-                         can_send_now = true;
+                         can_send_status();
                          PRINT_STR(fsm->type,"FSM_HOMING_BW: EVENT_DO reached endswitch\n");
                          
                        }
@@ -283,10 +283,8 @@ void fsm_homing_bw_zero(struct fsm *fsm, events my_event){
                            fsm->current_state = &fsm_homing_fw_5;
                          }
                          else 
-                         {
+                         {                                 /* PUSHER */
                            fsm->current_state = &fsm_stop;             // move to fsm_stop
-                           can_flags &= ~fsm->init_flag;
-                           can_send_now = true;
                            PRINT_STR(fsm->type,"fsm_homing_bw_zero: EVENT_DO reached endswitch\n");
                          }
                        }
@@ -326,18 +324,32 @@ void fsm_homing_fw_5(struct fsm *fsm, events my_event){
                break;
                
                case EVENT_EXIT:
-                   fsm_homing_block = FSM_PUSHER;              // release pusher
-                   can_flags &= ~fsm->init_flag;
                        break;
                        default:break;
        }
 }
 
+void fsm_wait_for_pusher_init(struct fsm *fsm, events my_event){
+       switch (my_event) {
+               case EVENT_ENTRY:
+                       can_flags &= ~fsm->init_flag;
+                       break;
+               case EVENT_DO:
+                       if ((can_flags & LIFT_PUSHER_INIT) == 0) {
+                                 fsm->current_state = &fsm_stop;       // move to fsm_homing_bw
+                       }
+                       break;
+               default:;
+       }
+}
+
 
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 void fsm_stop(struct fsm *fsm, events my_event){
        switch(my_event){
                case EVENT_ENTRY:
+                 can_flags &= ~fsm->init_flag;
+                 can_send_status();
                  fsm->set_engine(fsm->motor_dir , ENG_STOP);
                  PRINT_STR(fsm->type,"FSM_STOP: EVENT_ENTRY  \n");
                break;  
@@ -397,7 +409,7 @@ void fsm_move(struct fsm *fsm, events my_event){
                        fsm->set_engine(fsm->motor_dir, ENG_STOP);
                        fsm->current_state = &fsm_stop; // move to fsm_homing_bw
                //      fsm->ans_can = fsm->act_pos;
-                       can_send_now = true;
+                       can_send_status();
                        if(fsm->motor_dir == ENGINE_DIR_FW) 
                          can_flags |= fsm->endsw_flag;
                        PRINT_STR(fsm->type,"fsm_move: EVENT_DO reached endswitch \n");
@@ -422,8 +434,7 @@ void fsm_move(struct fsm *fsm, events my_event){
                          fsm->set_engine(fsm->motor_dir, ENG_STOP);
                          fsm->current_state = &fsm_wait;       // move to fsm_homing_bw
                          fsm->ans_can = fsm->req_pos;
-                         can_send_now = true;
-                         can_send_now = true;
+                         can_send_status();
                          PRINT_STR(fsm->type,"fsm_move: EVENT_DO reached target \n");
                  }
                  break;
@@ -444,6 +455,7 @@ void fsm_wait(struct fsm *fsm, events my_event){
                        
                case EVENT_DO:
                  
+                  fsm->ans_can = fsm->act_pos;
                  if((fsm->timeout_glob + TIME_WAIT) >+ time_ms)
                  {
                    PRINT_STR(fsm->type,"FSM_WAIT: EVENT_DO wait time done \n");

diff --git a/src/eb_lift_09/main.c b/src/eb_lift_09/main.c
index 63d78c4c85cac8b08b20aeb5197d7dad1aed95d9..418164b4029a0f9d1a4dd5a5ada43f69b78afe53 100644 (file)
--- a/src/eb_lift_09/main.c
+++ b/src/eb_lift_09/main.c
@@ -277,7 +277,6 @@ int main(void){
 // ~~~~~ initialization of periphery ~~~~~
 
        can_flags = 0;
-       can_send_now = false;
        init_periphery();
        
        fsm_pusher.type = FSM_PUSHER;
@@ -326,8 +325,7 @@ int main(void){
                //    can_send_db();
                }
                
-               if(can_send_now || timer_msec >= CAN_time + 100){       //repeat sending message every 100 ms
-                       can_send_now = false;
+               if (timer_msec >= CAN_time + 100){      //repeat sending message every 100 ms
                        CAN_time = timer_msec;          //save new time, when message was sent
                        can_send_status();
                }