summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/guybrush/bottom_clamp.c
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-14 10:56:00 +0200
committerNicolas Schodet2012-05-14 10:56:00 +0200
commita20005b026befa8499c1fca9f464e2d0300700ce (patch)
treeb426ad491dbbe0f8afd845a15edcbb7063e526fa /digital/io-hub/src/guybrush/bottom_clamp.c
parent2f89b61f644303d832c674912908c941b1fc8c9b (diff)
digital/io-hub/src/guybrush: add clamp_request to put a end to sync problems
Diffstat (limited to 'digital/io-hub/src/guybrush/bottom_clamp.c')
-rw-r--r--digital/io-hub/src/guybrush/bottom_clamp.c61
1 files changed, 57 insertions, 4 deletions
diff --git a/digital/io-hub/src/guybrush/bottom_clamp.c b/digital/io-hub/src/guybrush/bottom_clamp.c
index 0b951138..f801a78e 100644
--- a/digital/io-hub/src/guybrush/bottom_clamp.c
+++ b/digital/io-hub/src/guybrush/bottom_clamp.c
@@ -213,6 +213,8 @@ struct clamp_t
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;
+ /** Request from top FSM to be processed once in IDLE state. */
+ uint16_t idle_request;
};
/*Global context. */
@@ -236,6 +238,18 @@ void clamp_calm_mode(int mode)
ctx.calm_mode=mode;
}
+void
+clamp_request (uint16_t event)
+{
+ /* If event can not be handled right now, keep it until going in IDLE
+ * state. */
+ if (!FSM_HANDLE_VAR (AI, event))
+ {
+ assert (!ctx.idle_request);
+ ctx.idle_request = event;
+ }
+}
+
uint8_t clamp_read_blocked_cpt(void)
{
return ctx.cpt_blocked;
@@ -312,17 +326,56 @@ FSM_TRANS (CLAMP_GOING_IDLE, lower_clamp_rotation_success, CLAMP_WAIT_BEFORE_IDL
/*---------------------------------------------------------*/
/* IDLE!! */
/*---------------------------------------------------------*/
-FSM_TRANS_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, TIMEOUT_IDLE, CLAMP_IDLE)
+FSM_TRANS_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, TIMEOUT_IDLE,
+ request_clean_start, CLAMP_READY_TO_CLEAN,
+ request_tree_detected, CLAMP_BOTTOM_CLAMP_HIDE_POS,
+ idle, CLAMP_IDLE)
{
- /*Going back to the idle position, ready for showtime.*/
- fsm_queue_post_event (FSM_EVENT (AI, clamps_ready));
- return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE);
+ if (ctx.idle_request)
+ {
+ uint16_t idle_request = ctx.idle_request;
+ ctx.idle_request = 0;
+ if (idle_request == FSM_EVENT (AI, clean_start))
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, clamps_ready));
+ return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE,
+ request_clean_start);
+ }
+ else if (idle_request == FSM_EVENT (AI, tree_detected))
+ {
+ /* Hidding the clamp inside the robot. */
+ if (ctx.clamp_1_down)
+ {
+ move_needed (HIDE_POS_TREE * 250, FAST_ROTATION);
+ }
+ else
+ {
+ move_needed ((HALF_TURN + HIDE_POS_TREE) * 250, FAST_ROTATION);
+ ctx.clamp_1_down = 1;
+ }
+ return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE,
+ request_tree_detected);
+ }
+ else
+ {
+ assert (0);
+ return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, idle);
+ }
+ }
+ else
+ {
+ /* Going back to the idle position, ready for showtime. */
+ fsm_queue_post_event (FSM_EVENT (AI, clamps_ready));
+ return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, idle);
+ }
}
+
/*---------------------------------------------------------*/
/* parts of the FSM that Clean areas */
/*---------------------------------------------------------*/
FSM_TRANS (CLAMP_IDLE, clean_start,CLAMP_READY_TO_CLEAN)
{
+ fsm_queue_post_event (FSM_EVENT (AI, clamps_ready));
return FSM_NEXT(CLAMP_IDLE,clean_start);
}