From: pokormat <matous.pokorny@me.com>
Date: Sun, 1 May 2011 08:04:40 +0000 (+0200)
Subject: eb_lift_11: add end switch function
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/259e9586d7062b1d919d05b9bd8c28cb1c138793

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 05866665..d0c01435 100644
--- 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;