From c7a877463c349faf557957479725a7ab53706c48 Mon Sep 17 00:00:00 2001 From: Julien Balleyguier Date: Sun, 13 May 2012 12:50:33 +0200 Subject: digital/io-hub/src/guybrush/bottom_clamp.c : modifying the blocked sequence --- digital/io-hub/src/guybrush/bottom_clamp.c | 32 +++++++++++++++++++++++++++--- 1 file changed, 29 insertions(+), 3 deletions(-) (limited to 'digital') diff --git a/digital/io-hub/src/guybrush/bottom_clamp.c b/digital/io-hub/src/guybrush/bottom_clamp.c index 1e6f2d4c..5bb264a8 100644 --- a/digital/io-hub/src/guybrush/bottom_clamp.c +++ b/digital/io-hub/src/guybrush/bottom_clamp.c @@ -134,8 +134,12 @@ FSM_EVENTS ( clamps_ready, /* the robot is ready to empty the tree, and the top fsm is asking him to do so*/ empty_tree, + /* The robot has step back. Event given by the main.c programm(after an empty tree, or a clamp_block event. */ robot_is_back, - clamp_blocked + /* The clamp is blocked. We can try to unblock it*/ + clamp_blocked, + /* We tryed to unblock the clamp too many time. we can now say that the bottom clamp is out of order*/ + clamp_is_dead ) @@ -178,6 +182,8 @@ struct clamp_t uint8_t stop_tree_approach; /** True if we are in "Calm mod" (clamps can't move)*/ uint8_t calm_mode; + /** Compting the number of time the bottom clamp is being blocked in a row (reset when going back to idle)*/ + uint8_t cpt_blocked; }; /*Global context. */ @@ -201,6 +207,11 @@ void clamp_calm_mode(int mode) ctx.calm_mode=mode; } +uint8_t clamp_read_blocked_cpt(void) +{ + return ctx.cpt_blocked; +} + /*---------------------------------------------------------*/ /* INIT part of the FSM */ @@ -574,11 +585,22 @@ FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS,robot_is_back, CLAMP_WAIT) return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, robot_is_back); } +FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS,clamp_is_dead, CLAMP_SHITTY_STATE) +{ + return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, clamp_is_dead); +} + FSM_TRANS (CLAMP_WAIT,lower_clamp_rotation_success, CLAMP_TURN_BACKWARD) { return FSM_NEXT (CLAMP_WAIT, lower_clamp_rotation_success); } +FSM_TRANS (CLAMP_WAIT,lower_clamp_rotation_failure, CLAMP_BLOCKED) +{ + ctx.cpt_blocked += 1; + return FSM_NEXT (CLAMP_WAIT, lower_clamp_rotation_failure); +} + FSM_TRANS_TIMEOUT (CLAMP_TURN_BACKWARD,TIMEOUT_BLOCKED, CLAMP_TURN_FORWARD) { move_needed(0,FAILURE_ROTATION); @@ -598,18 +620,22 @@ FSM_TRANS_TIMEOUT (CLAMP_TURN_BACKWARD,TIMEOUT_BLOCKED, CLAMP_TURN_FORWARD) FSM_TRANS (CLAMP_TURN_FORWARD,lower_clamp_rotation_success, CLAMP_IDLE) { + ctx.cpt_blocked = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamps_ready)); return FSM_NEXT (CLAMP_TURN_FORWARD, lower_clamp_rotation_success); } -FSM_TRANS (CLAMP_TURN_BACKWARD,lower_clamp_rotation_failure, CLAMP_SHITTY_STATE) +FSM_TRANS (CLAMP_TURN_BACKWARD,lower_clamp_rotation_failure, CLAMP_BLOCKED) { + ctx.cpt_blocked += 1; return FSM_NEXT (CLAMP_TURN_BACKWARD, lower_clamp_rotation_failure); } -FSM_TRANS (CLAMP_TURN_FORWARD,lower_clamp_rotation_failure, CLAMP_SHITTY_STATE) +FSM_TRANS (CLAMP_TURN_FORWARD,lower_clamp_rotation_failure, CLAMP_BLOCKED) { + ctx.cpt_blocked += 1; return FSM_NEXT (CLAMP_TURN_FORWARD, lower_clamp_rotation_failure); } -- cgit v1.2.3