eb_lift_11: add end switch function

diff --git a/src/eb_lift_11/fsm_lift.c b/src/eb_lift_11/fsm_lift.c
index 058666657059b6fdea08c40afb50e4a66307efa1..d0c01435c08abea6815e44fad15f4ece09ad5b09 100644 (file)
--- a/src/eb_lift_11/fsm_lift.c
+++ b/src/eb_lift_11/fsm_lift.c
@@ -142,6 +142,7 @@ static void wait_for_cmd(struct fsm *fsm, enum event event)
 static void move(struct fsm *fsm, enum event event)
 {
        static int counter;
+        static bool lift_stopped_on_end = false;
        bool finished;
        switch (event) {
        case EVENT_ENTRY:
@@ -155,6 +156,30 @@ static void move(struct fsm *fsm, enum event event)
                        fsm->req_pos = fsm->start_pos;
                break;
        case EVENT_DO:
+                if ((fsm->flags & CAN_LIFT_SWITCH_UP) && (fsm->can_response != fsm->req_target)){
+                       if(!lift_stopped_on_end){
+                          fsm->can_response = fsm->req_target;
+                          fsm->current_state = wait_for_cmd;
+                          fsm->req_pos = fsm->act_pos; 
+                          lift_stopped_on_end=true;
+                       }
+                       else{
+                         lift_stopped_on_end=false;
+                      }
+                }
+                if ((fsm->flags & CAN_LIFT_SWITCH_DOWN) && (fsm->can_response != fsm->req_target)) {
+                       if(!lift_stopped_on_end){ 
+                          fsm->can_response = fsm->req_target;
+                          fsm->current_state = wait_for_cmd;
+                          fsm->act_pos=0;
+                          fsm->req_pos = fsm->act_pos;    
+                          lift_stopped_on_end=true;
+                       }
+                       else{
+                         lift_stopped_on_end=false;
+                      }
+                        
+                }
                if (fsm->can_req_position != fsm->req_target) {
                        fsm->flags |= CAN_LIFT_TIMEOUT;
                        fsm->current_state = wait_for_cmd;