summaryrefslogtreecommitdiff
path: root/digital/io-hub/src
diff options
context:
space:
mode:
authorJulien Balleyguier2012-05-13 12:50:33 +0200
committerJulien Balleyguier2012-05-13 13:25:26 +0200
commitc7a877463c349faf557957479725a7ab53706c48 (patch)
treef0138c567cf5a331e4ddecc19870f8a015725d36 /digital/io-hub/src
parent024d0b04e63c0a60ec8d0e7a11f9414f5abfc7ec (diff)
digital/io-hub/src/guybrush/bottom_clamp.c : modifying the blocked sequence
Diffstat (limited to 'digital/io-hub/src')
-rw-r--r--digital/io-hub/src/guybrush/bottom_clamp.c32
1 files changed, 29 insertions, 3 deletions
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);
}