summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-07 14:36:30 +0200
committerNicolas Schodet2012-05-07 16:31:47 +0200
commit056213a6203beb480857318420ff099e9210e806 (patch)
treef1d2a5e0cb12de1ee2703716e6f5a3cb782c9af4
parentc82623b6b12153e697e2226a56ce44773875a3d0 (diff)
digital/io-hub/src/guybrush: interract with clamp FSM
-rw-r--r--digital/io-hub/src/guybrush/top.c37
1 files changed, 31 insertions, 6 deletions
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 5de7dc85..f003e3f7 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -30,6 +30,7 @@
#define FSM_NAME AI
#include "fsm.h"
+#include "fsm_queue.h"
#include "move.h"
#include "chrono.h"
@@ -53,12 +54,18 @@ FSM_STATES (
/* Going to a collect position above or below a totem. */
TOP_TOTEM_GOING,
+ /* Put clamps down. */
+ TOP_TOTEM_CLAMP_DOWNING,
/* Approaching a totem. */
TOP_TOTEM_APPROACHING,
+ /* Emptying tree. */
+ TOP_TOTEM_EMPTYING,
/* Pushing until full contact. */
TOP_TOTEM_PUSHING,
/* Going back after totem has been emptied. */
TOP_TOTEM_GOING_BACK,
+ /* Put clamps up. */
+ TOP_TOTEM_CLAMP_UPPING,
/* Going to push a bottle. */
TOP_BOTTLE_GOING,
@@ -164,32 +171,50 @@ FSM_TRANS (TOP_START, init_start_round,
/** TOTEM */
-FSM_TRANS (TOP_TOTEM_GOING, move_success, TOP_TOTEM_APPROACHING)
+FSM_TRANS (TOP_TOTEM_GOING, move_success, TOP_TOTEM_CLAMP_DOWNING)
{
- asserv_move_linearly (PATH_GRID_CLEARANCE_MM - BOT_SIZE_FRONT - 30);
+ fsm_queue_post_event (FSM_EVENT (AI, tree_detected));
return FSM_NEXT (TOP_TOTEM_GOING, move_success);
}
+FSM_TRANS (TOP_TOTEM_CLAMP_DOWNING, clamps_ready, TOP_TOTEM_APPROACHING)
+{
+ asserv_move_linearly (PATH_GRID_CLEARANCE_MM - BOT_SIZE_FRONT - 30);
+ return FSM_NEXT (TOP_TOTEM_CLAMP_DOWNING, clamps_ready);
+}
+
FSM_TRANS (TOP_TOTEM_APPROACHING, robot_move_success, TOP_TOTEM_PUSHING)
{
asserv_push_the_wall (0, -1, -1, -1);
return FSM_NEXT (TOP_TOTEM_APPROACHING, robot_move_success);
}
-FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_success, TOP_TOTEM_GOING_BACK)
+FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_success, TOP_TOTEM_EMPTYING)
{
asserv_stop_motor ();
+ fsm_queue_post_event (FSM_EVENT (AI, empty_tree));
+ return FSM_NEXT (TOP_TOTEM_PUSHING, robot_move_success);
+}
+
+FSM_TRANS (TOP_TOTEM_EMPTYING, clamps_ready, TOP_TOTEM_GOING_BACK)
+{
strat_success ();
move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
- return FSM_NEXT (TOP_TOTEM_PUSHING, robot_move_success);
+ return FSM_NEXT (TOP_TOTEM_EMPTYING, clamps_ready);
+}
+
+FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success, TOP_TOTEM_CLAMP_UPPING)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, robot_is_back));
+ return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_success);
}
-FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success,
+FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamps_ready,
totem, TOP_TOTEM_GOING,
bottle, TOP_BOTTLE_GOING,
unload, TOP_UNLOAD_GOING)
{
- RETURN_TOP_DECISION_SWITCH (TOP_TOTEM_GOING_BACK, move_success);
+ RETURN_TOP_DECISION_SWITCH (TOP_TOTEM_CLAMP_UPPING, clamps_ready);
}
/** BOTTLE */