eb_lift_11: add state homing

diff --git a/src/eb_lift_11/fsm_lift.c b/src/eb_lift_11/fsm_lift.c
index ad15c3dd953466c7e6120dc085352055ab4875b4..058666657059b6fdea08c40afb50e4a66307efa1 100644 (file)
--- a/src/eb_lift_11/fsm_lift.c
+++ b/src/eb_lift_11/fsm_lift.c
@@ -17,38 +17,53 @@
 
 static void wait_for_cmd(struct fsm *fsm, enum event event);
 static void move(struct fsm *fsm, enum event event);
+static void homing(struct fsm *fsm, enum event event);
 
 
-void fsm_lift_init(struct fsm *fsm, enum event event)
+static void homing(struct fsm *fsm, enum event event)
 {
-       static uint32_t time_start=0;
-       
+        static uint32_t time_start=0;
+        
+        switch (event) {
+        case EVENT_ENTRY:
+                DBG_ENTRY();
+                time_start=timer_msec;
+                break;
+        case EVENT_DO:
+                //homing with 3s timeout
+                engine_A_dir(ENGINE_DIR_FW);
+                engine_A_pwm(50);
+                
+                if(fsm->flags & CAN_LIFT_SWITCH_DOWN){
+                  engine_A_pwm(0);
+                  fsm->act_pos=0;
+                  fsm->current_state = wait_for_cmd;  
+                  fsm->flags |= CAN_LIFT_HOMED;
+//                   fsm->can_req_position = 0x54E2;
+                }
+                else if (timer_msec >= (time_start + 3000)) {
+                  engine_A_pwm(0);
+                  fsm->current_state = wait_for_cmd;
+                  fsm->flags |= CAN_LIFT_TIMEOUT;
+                  fsm->flags &= ~CAN_LIFT_HOMED;
+                  fsm->trigger_can_send=1;
+                }
+                                
+                break;
+        case EVENT_EXIT:
+                break;
+        }
+}
+
+void fsm_lift_init(struct fsm *fsm, enum event event)
+{      
        switch (event) {
        case EVENT_ENTRY:
                DBG_ENTRY();
                fsm->flags |= CAN_LIFT_INITIALIZING;
-               time_start=timer_msec;
                break;
        case EVENT_DO:
-               //homing with 3s timeout
                fsm->flags &= ~CAN_LIFT_INITIALIZING;
-               engine_A_dir(ENGINE_DIR_BW);
-               engine_A_pwm(50);
-               
-               if(IO0PIN & (1<<END_SWITCH_UP)){
-                 engine_A_pwm(0);
-                 fsm->act_pos=0;
-                 fsm->trigger_can_send=1;
-                 fsm->current_state = wait_for_cmd;    
-                  fsm->can_req_position = 0x54E2;
-               }
-               else if (timer_msec >= (time_start + 3000)) {
-                 engine_A_pwm(0);
-                 fsm->current_state = wait_for_cmd;
-                 fsm->flags &= CAN_LIFT_TIMEOUT;
-                  fsm->trigger_can_send=1;
-               }
-                               
                break;
        case EVENT_EXIT:
                break;
@@ -100,6 +115,14 @@ static void wait_for_cmd(struct fsm *fsm, enum event event)
                stop();
                break;
        case EVENT_DO:
+                //homing if push home button 
+                if (fsm->flags & CAN_LIFT_SWITCH_HOME){
+                  fsm->current_state=homing;
+                }
+                //homing if match start, but did not homing before start
+                if ((fsm->flags & CAN_LIFT_START) && !(fsm->flags & CAN_LIFT_HOMED)){
+                  fsm->current_state=homing;
+                }
                do_control(fsm, 2);
                if (fsm->can_req_position != last_can_req_pos &&
                    fsm->can_req_position != fsm->req_target) {