summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io-hub/src/guybrush/bottom_clamp.c61
-rw-r--r--digital/io-hub/src/guybrush/bottom_clamp.h4
-rw-r--r--digital/io-hub/src/guybrush/top.c47
3 files changed, 71 insertions, 41 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);
}
diff --git a/digital/io-hub/src/guybrush/bottom_clamp.h b/digital/io-hub/src/guybrush/bottom_clamp.h
index c3cd773a..240b1579 100644
--- a/digital/io-hub/src/guybrush/bottom_clamp.h
+++ b/digital/io-hub/src/guybrush/bottom_clamp.h
@@ -31,5 +31,9 @@ void clamp_calm_mode(int mode);
/*function to find out how many times the clamp has been blocked*/
uint8_t clamp_read_blocked_cpt(void);
+/** Request a specific move to be executed now or as soon as going to IDLE
+ * state. */
+void
+clamp_request (uint16_t event);
#endif /* bottom_clamp_h */
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 0fcf9b03..0b763480 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -37,6 +37,7 @@
#include "strat.h"
#include "path.h"
+#include "bottom_clamp.h"
#include "output_defs.h"
@@ -58,7 +59,7 @@ FSM_STATES (
/* Going to a collect position above or below a totem. */
TOP_TOTEM_GOING,
/* Cleaning: waiting clamp FSM. */
- TOP_TOTEM_CLEAN_WAITING,
+ TOP_TOTEM_CLEAN_STARTING,
/* Cleaning: approaching totem. */
TOP_TOTEM_CLEAN_APPROACHING,
/* Cleaning; closing clamps. */
@@ -67,8 +68,6 @@ FSM_STATES (
TOP_TOTEM_CLEAN_GOING_BACK,
/* Cleaning: loading. */
TOP_TOTEM_CLEAN_LOADING,
- /* Waiting clamp FSM to put clamps down. */
- TOP_TOTEM_CLAMP_WAITING,
/* Put clamps down. */
TOP_TOTEM_CLAMP_DOWNING,
/* Approaching a totem. */
@@ -204,27 +203,16 @@ FSM_TRANS (TOP_INIT, init_start_round,
/** TOTEM */
-FSM_TRANS (TOP_TOTEM_GOING, move_success,
- wait, TOP_TOTEM_CLEAN_WAITING,
- ready, TOP_TOTEM_CLEAN_APPROACHING)
+FSM_TRANS (TOP_TOTEM_GOING, move_success, TOP_TOTEM_CLEAN_STARTING)
{
- if (!FSM_CAN_HANDLE (AI, clean_start))
- {
- return FSM_NEXT (TOP_TOTEM_GOING, move_success, wait);
- }
- else
- {
- FSM_HANDLE (AI, clean_start);
- asserv_move_linearly (PATH_GRID_CLEARANCE_MM - BOT_SIZE_FRONT - 130);
- return FSM_NEXT (TOP_TOTEM_GOING, move_success, ready);
- }
+ clamp_request (FSM_EVENT (AI, clean_start));
+ return FSM_NEXT (TOP_TOTEM_GOING, move_success);
}
-FSM_TRANS (TOP_TOTEM_CLEAN_WAITING, clamps_ready, TOP_TOTEM_CLEAN_APPROACHING)
+FSM_TRANS (TOP_TOTEM_CLEAN_STARTING, clamps_ready, TOP_TOTEM_CLEAN_APPROACHING)
{
- FSM_HANDLE (AI, clean_start);
asserv_move_linearly (PATH_GRID_CLEARANCE_MM - BOT_SIZE_FRONT - 130);
- return FSM_NEXT (TOP_TOTEM_CLEAN_WAITING, clamps_ready);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_STARTING, clamps_ready);
}
FSM_TRANS (TOP_TOTEM_CLEAN_APPROACHING, robot_move_success,
@@ -248,25 +236,10 @@ FSM_TRANS (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_success,
return FSM_NEXT (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_success);
}
-FSM_TRANS (TOP_TOTEM_CLEAN_LOADING, clamps_ready,
- wait, TOP_TOTEM_CLAMP_WAITING,
- ready, TOP_TOTEM_CLAMP_DOWNING)
-{
- if (!FSM_CAN_HANDLE (AI, tree_detected))
- {
- return FSM_NEXT (TOP_TOTEM_CLEAN_LOADING, clamps_ready, wait);
- }
- else
- {
- FSM_HANDLE (AI, tree_detected);
- return FSM_NEXT (TOP_TOTEM_CLEAN_LOADING, clamps_ready, ready);
- }
-}
-
-FSM_TRANS (TOP_TOTEM_CLAMP_WAITING, clamps_ready, TOP_TOTEM_CLAMP_DOWNING)
+FSM_TRANS (TOP_TOTEM_CLEAN_LOADING, clamps_ready, TOP_TOTEM_CLAMP_DOWNING)
{
- FSM_HANDLE (AI, tree_detected);
- return FSM_NEXT (TOP_TOTEM_CLAMP_WAITING, clamps_ready);
+ clamp_request (FSM_EVENT (AI, tree_detected));
+ return FSM_NEXT (TOP_TOTEM_CLEAN_LOADING, clamps_ready);
}
FSM_TRANS (TOP_TOTEM_CLAMP_DOWNING, clamps_ready, TOP_TOTEM_APPROACHING)