summaryrefslogtreecommitdiff
path: root/digital/io-hub/src
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub/src')
-rw-r--r--digital/io-hub/src/guybrush/Makefile2
-rw-r--r--digital/io-hub/src/guybrush/bottom_clamp.c140
-rw-r--r--digital/io-hub/src/guybrush/main.c26
-rw-r--r--digital/io-hub/src/guybrush/move.c82
-rw-r--r--digital/io-hub/src/guybrush/top.c75
5 files changed, 95 insertions, 230 deletions
diff --git a/digital/io-hub/src/guybrush/Makefile b/digital/io-hub/src/guybrush/Makefile
index 0c642017..e5669ce9 100644
--- a/digital/io-hub/src/guybrush/Makefile
+++ b/digital/io-hub/src/guybrush/Makefile
@@ -32,7 +32,7 @@ vpath %.h $(AI_MODULES:%=../../../ai/src/%)
INCLUDES += -I. -I../common $(AI_MODULES:%=-I../../../ai/src/%)
EXTRA_CLEAN_FILES = angfsm_gen_avr_AI.h fsm_AI_gen.avr.c \
- dot_AI_1.dot dot_AI_1.png dot_AI_1.svg
+ AI.dot AI.png AI.svg
include $(BASE)/make/Makefile.gen
diff --git a/digital/io-hub/src/guybrush/bottom_clamp.c b/digital/io-hub/src/guybrush/bottom_clamp.c
index e6b844b6..e3e4978a 100644
--- a/digital/io-hub/src/guybrush/bottom_clamp.c
+++ b/digital/io-hub/src/guybrush/bottom_clamp.c
@@ -333,9 +333,6 @@ FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPEN_ALL_CLAMPS)
IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
/*Opening upper clamps*/
IO_SET (OUTPUT_UPPER_CLAMP_OPEN);
-
- return FSM_NEXT (CLAMP_START,init_actuators);
-
}
FSM_TRANS_TIMEOUT (CLAMP_INIT_OPEN_ALL_CLAMPS,TIMEOUT_OPEN_CLAMPS,
@@ -348,12 +345,12 @@ FSM_TRANS_TIMEOUT (CLAMP_INIT_OPEN_ALL_CLAMPS,TIMEOUT_OPEN_CLAMPS,
/* closing all the clamps except the clamp 1 to proceed the find_O function. */
IO_SET (OUTPUT_LOWER_CLAMP_2_CLOSE);
IO_CLR (OUTPUT_UPPER_CLAMP_OPEN);
- return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPEN_ALL_CLAMPS,upper_set_in);
+ return FSM_BRANCH (upper_set_in);
}
/*if it is outside, go to shitty state and stop everything*/
else
{
- return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPEN_ALL_CLAMPS,upper_set_out);
+ return FSM_BRANCH (upper_set_out);
}
@@ -368,7 +365,6 @@ FSM_TRANS_TIMEOUT (CLAMP_INIT_CLOSE_ALL_CLAMPS, 5*TIMEOUT_OPEN_CLAMPS, CLAMP_INI
ctx.unblocking_upper_set = 0;
ctx.upper_set_is_dead = 0;
move_needed2(8000,SLOW_ROTATION,1);
- return FSM_NEXT_TIMEOUT (CLAMP_INIT_CLOSE_ALL_CLAMPS);
}
FSM_TRANS (CLAMP_INIT_FIND_0, 0_found, CLAMP_INIT_HIDE_CLAMP)
@@ -379,7 +375,6 @@ FSM_TRANS (CLAMP_INIT_FIND_0, 0_found, CLAMP_INIT_HIDE_CLAMP)
/* Robot is ready with clamp 1 at the bottom. */
move_needed2(CLAMP_1_DOWN,FAST_ROTATION,1);
ctx.clamp_1_down = 1;
- return FSM_NEXT (CLAMP_INIT_FIND_0, 0_found);
}
FSM_TRANS (CLAMP_INIT_HIDE_CLAMP, lower_clamp_rotation_success,
@@ -390,14 +385,13 @@ FSM_TRANS (CLAMP_INIT_HIDE_CLAMP, lower_clamp_rotation_success,
IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
/*Looking if we are in demo mode or normal mode*/
if (!main_demo)
- return FSM_NEXT (CLAMP_INIT_HIDE_CLAMP,lower_clamp_rotation_success,normal_start);
+ return FSM_BRANCH (normal_start);
else
- return FSM_NEXT (CLAMP_INIT_HIDE_CLAMP,lower_clamp_rotation_success,demo_start);
+ return FSM_BRANCH (demo_start);
}
FSM_TRANS (CLAMP_INIT_READY,init_start_round, CLAMP_GOING_IDLE)
{
- return FSM_NEXT (CLAMP_INIT_READY, init_start_round);
}
FSM_TRANS_TIMEOUT (CLAMP_GOING_IDLE, 3*TIMEOUT_OPEN_CLAMPS, CLAMP_WAIT_BEFORE_IDLE)
@@ -411,7 +405,6 @@ FSM_TRANS_TIMEOUT (CLAMP_GOING_IDLE, 3*TIMEOUT_OPEN_CLAMPS, CLAMP_WAIT_BEFORE_ID
IO_CLR (OUTPUT_UPPER_CLAMP_IN);
/*Contrepression*/
IO_SET (OUTPUT_UPPER_CLAMP_UP);
- return FSM_NEXT_TIMEOUT (CLAMP_GOING_IDLE);
}
/*---------------------------------------------------------*/
@@ -426,13 +419,12 @@ FSM_TRANS_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, TIMEOUT_IDLE,
{
uint16_t idle_request = ctx.idle_request;
ctx.idle_request = 0;
- if (idle_request == FSM_EVENT (AI, clean_start))
+ if (idle_request == FSM_EVENT (clean_start))
{
- fsm_queue_post_event (FSM_EVENT (AI, clamps_ready));
- return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE,
- request_clean_start);
+ fsm_queue_post_event (FSM_EVENT (clamps_ready));
+ return FSM_BRANCH (request_clean_start);
}
- else if (idle_request == FSM_EVENT (AI, tree_detected))
+ else if (idle_request == FSM_EVENT (tree_detected))
{
/* Hidding the clamp inside the robot. */
if (is_clamp_1_down(ctx.current_pos))
@@ -444,20 +436,19 @@ FSM_TRANS_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, TIMEOUT_IDLE,
move_needed2 (HIDE_POS_TREE, FAST_ROTATION,1);
ctx.clamp_1_down = 1;
}
- return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE,
- request_tree_detected);
+ return FSM_BRANCH (request_tree_detected);
}
else
{
assert (0);
- return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, idle);
+ return FSM_BRANCH (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);
+ fsm_queue_post_event (FSM_EVENT (clamps_ready));
+ return FSM_BRANCH (idle);
}
}
/*---------------------------------------------------------*/
@@ -465,8 +456,7 @@ FSM_TRANS_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, TIMEOUT_IDLE,
/*---------------------------------------------------------*/
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);
+ fsm_queue_post_event (FSM_EVENT (clamps_ready));
}
FSM_TRANS (CLAMP_READY_TO_CLEAN, clean_catch,CLAMP_CATCH_COIN)
@@ -482,17 +472,14 @@ FSM_TRANS (CLAMP_READY_TO_CLEAN, clean_catch,CLAMP_CATCH_COIN)
{
IO_SET (OUTPUT_LOWER_CLAMP_2_CLOSE);
}
- return FSM_NEXT(CLAMP_READY_TO_CLEAN,clean_catch);
}
FSM_TRANS_TIMEOUT (CLAMP_CATCH_COIN, TIMEOUT_CLOSE_CLAMPS*2,CLAMP_READY_TO_LOAD)
{
- fsm_queue_post_event (FSM_EVENT (AI, clamps_ready));
- return FSM_NEXT_TIMEOUT(CLAMP_CATCH_COIN);
+ fsm_queue_post_event (FSM_EVENT (clamps_ready));
}
FSM_TRANS (CLAMP_READY_TO_LOAD, clean_load,CLAMP_TAKE_COIN)
{
- return FSM_NEXT(CLAMP_READY_TO_LOAD,clean_load);
}
/*---------------------------------------------------------*/
@@ -511,8 +498,7 @@ FSM_TRANS (CLAMP_IDLE, coin_detected,CLAMP_TAKE_COIN)
{
IO_SET(OUTPUT_LOWER_CLAMP_2_CLOSE);
}
- fsm_queue_post_event (FSM_EVENT (AI, taking_coin));
- return FSM_NEXT (CLAMP_IDLE, coin_detected);
+ fsm_queue_post_event (FSM_EVENT (taking_coin));
}
FSM_TRANS_TIMEOUT (CLAMP_TAKE_COIN, TIMEOUT_CLOSE_CLAMPS, CLAMP_TURN_HALF_WAY)
@@ -527,7 +513,6 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKE_COIN, TIMEOUT_CLOSE_CLAMPS, CLAMP_TURN_HALF_WAY)
move_needed2(CLAMP_1_DOWN,FAST_ROTATION,1);
}
- return FSM_NEXT_TIMEOUT (CLAMP_TAKE_COIN);
}
FSM_TRANS (CLAMP_TURN_HALF_WAY, time_to_drop_coin, CLAMP_DROP_CD)
@@ -543,24 +528,20 @@ FSM_TRANS (CLAMP_TURN_HALF_WAY, time_to_drop_coin, CLAMP_DROP_CD)
{
IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
}
- return FSM_NEXT (CLAMP_TURN_HALF_WAY,time_to_drop_coin);
}
FSM_TRANS (CLAMP_TURN_HALF_WAY, lower_clamp_rotation_failure, CLAMP_BLOCKED)
{
- return FSM_NEXT (CLAMP_TURN_HALF_WAY,lower_clamp_rotation_failure);
}
FSM_TRANS (CLAMP_DROP_CD,lower_clamp_rotation_success,CLAMP_WAIT_BEFORE_IDLE)
{
- return FSM_NEXT (CLAMP_DROP_CD,lower_clamp_rotation_success);
}
FSM_TRANS (CLAMP_DROP_CD, lower_clamp_rotation_failure, CLAMP_BLOCKED)
{
/*The clamp is blocked somehow. */
clamp_blocked();
- return FSM_NEXT (CLAMP_DROP_CD, lower_clamp_rotation_failure);
}
/*---------------------------------------------------------*/
/* Parts of the FSM that is Approaching the tree (totem) */
@@ -579,7 +560,6 @@ FSM_TRANS (CLAMP_IDLE, tree_detected,CLAMP_BOTTOM_CLAMP_HIDE_POS)
}
/*Hidding the clamp inside the robot*/
move_needed2(rotation,FAST_ROTATION,1);
- return FSM_NEXT (CLAMP_IDLE, tree_detected);
}
@@ -587,14 +567,12 @@ FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_success, CLAMP_UNFO
{
IO_CLR (OUTPUT_UPPER_CLAMP_UP);
IO_SET (OUTPUT_UPPER_CLAMP_DOWN);
- return FSM_NEXT (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_success);
}
FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure, CLAMP_BLOCKED)
{
/*The clamp is blocked somehow.*/
clamp_blocked();
- return FSM_NEXT (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure);
}
@@ -606,27 +584,23 @@ FSM_TRANS (CLAMP_UNFOLD_UPPER_SET, upper_set_down, CLAMP_BOTTOM_CLAMP_READY)
/*Opening the top clamp.*/
IO_SET (OUTPUT_UPPER_CLAMP_OPEN);
- return FSM_NEXT (CLAMP_UNFOLD_UPPER_SET, upper_set_down);
}
/** If the upper set doesn't go down*/
FSM_TRANS_TIMEOUT (CLAMP_UNFOLD_UPPER_SET, TIMEOUT_UPPER_SET_DOWN, CLAMP_UPPER_SET_BLOCKED_UP)
{
- return FSM_NEXT_TIMEOUT (CLAMP_UNFOLD_UPPER_SET);
}
FSM_TRANS (CLAMP_BOTTOM_CLAMP_READY, lower_clamp_rotation_success, CLAMP_READY_TO_EMPTY_TREE)
{
- fsm_queue_post_event (FSM_EVENT (AI, clamps_ready));
- return FSM_NEXT (CLAMP_BOTTOM_CLAMP_READY, lower_clamp_rotation_success);
+ fsm_queue_post_event (FSM_EVENT (clamps_ready));
}
FSM_TRANS (CLAMP_BOTTOM_CLAMP_READY, lower_clamp_rotation_failure, CLAMP_BLOCKED_UPPER_SET_DOWN)
{
- return FSM_NEXT (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure);
}
FSM_TRANS (CLAMP_READY_TO_EMPTY_TREE, empty_tree, CLAMP_CLOSE_ALL_CLAMPS)
@@ -640,8 +614,7 @@ FSM_TRANS (CLAMP_READY_TO_EMPTY_TREE, empty_tree, CLAMP_CLOSE_ALL_CLAMPS)
/*Closing upper & middle clamps.*/
IO_CLR (OUTPUT_UPPER_CLAMP_OPEN);
- fsm_queue_post_event (FSM_EVENT (AI, clamps_ready));
- return FSM_NEXT (CLAMP_READY_TO_EMPTY_TREE, empty_tree);
+ fsm_queue_post_event (FSM_EVENT (clamps_ready));
}
@@ -653,7 +626,6 @@ FSM_TRANS(CLAMP_CLOSE_ALL_CLAMPS, robot_is_back,CLAMP_REARRANGE_CD)
{
IO_CLR (OUTPUT_UPPER_CLAMP_OUT);
IO_SET (OUTPUT_UPPER_CLAMP_IN);
- return FSM_NEXT (CLAMP_CLOSE_ALL_CLAMPS, robot_is_back);
}
@@ -662,27 +634,23 @@ FSM_TRANS (CLAMP_CLOSE_ALL_CLAMPS, stop_tree_approach, CLAMP_READY_TO_EMPTY_TREE
IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
IO_SET (OUTPUT_UPPER_CLAMP_OPEN);
- fsm_queue_post_event (FSM_EVENT (AI, clamps_ready));
- return FSM_NEXT (CLAMP_CLOSE_ALL_CLAMPS, stop_tree_approach);
+ fsm_queue_post_event (FSM_EVENT (clamps_ready));
}
FSM_TRANS_TIMEOUT (CLAMP_REARRANGE_CD, TIMEOUT_RECENTRAGE, CLAMP_BOTTOM_CLAMP_HIDE_POS2)
{
/*Hidding the clamp inside the robot.*/
move_needed2(LET_UPPER_SET_UP,MEDIUM_ROTATION,1);
- return FSM_NEXT_TIMEOUT (CLAMP_REARRANGE_CD);
}
FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_success, CLAMP_RELEASE_ASSERV)
{
mimot_motor_free (0, 1);
- return FSM_NEXT (CLAMP_BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_success);
}
FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_failure, CLAMP_BLOCKED)
{
clamp_blocked();
- return FSM_NEXT (CLAMP_BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_failure);
}
@@ -690,7 +658,6 @@ FSM_TRANS_TIMEOUT (CLAMP_RELEASE_ASSERV, TIMEOUT_FREE_ASSERV, CLAMP_FOLD_UPPER_S
{
IO_CLR (OUTPUT_UPPER_CLAMP_DOWN);
IO_SET (OUTPUT_UPPER_CLAMP_UP);
- return FSM_NEXT_TIMEOUT (CLAMP_RELEASE_ASSERV);
}
FSM_TRANS (CLAMP_FOLD_UPPER_SET, upper_set_up, CLAMP_OPEN_UPPER_CLAMPS)
@@ -698,7 +665,6 @@ FSM_TRANS (CLAMP_FOLD_UPPER_SET, upper_set_up, CLAMP_OPEN_UPPER_CLAMPS)
IO_SET (OUTPUT_UPPER_CLAMP_OPEN);
IO_CLR (OUTPUT_UPPER_CLAMP_IN);
IO_SET (OUTPUT_UPPER_CLAMP_OUT);
- return FSM_NEXT (CLAMP_FOLD_UPPER_SET, upper_set_up);
}
@@ -715,42 +681,37 @@ FSM_TRANS_TIMEOUT (CLAMP_OPEN_UPPER_CLAMPS, TIMEOUT_OPEN_CLAMPS,
main_set_drop_coin_pos(ctx.current_pos + POS_TO_DROP + ctx.init_pos);
move_needed2(CLAMP_1_UP,FAST_ROTATION,1);
ctx.stop_tree_approach = 0;
- fsm_queue_post_event (FSM_EVENT (AI, clamps_ready));
- return FSM_NEXT_TIMEOUT (CLAMP_OPEN_UPPER_CLAMPS,stop_tree_branch);
+ fsm_queue_post_event (FSM_EVENT (clamps_ready));
+ return FSM_BRANCH (stop_tree_branch);
}
else
{
move_needed2(RECALE_COIN,MEDIUM_ROTATION,1);
- return FSM_NEXT_TIMEOUT (CLAMP_OPEN_UPPER_CLAMPS,continue_empty_tree_branch);
+ return FSM_BRANCH (continue_empty_tree_branch);
}
}
FSM_TRANS (CLAMP_READY_TO_RECALE, lower_clamp_rotation_success, CLAMP_READY_TO_RECALE_2)
{
IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
- return FSM_NEXT (CLAMP_READY_TO_RECALE,lower_clamp_rotation_success);
}
FSM_TRANS (CLAMP_READY_TO_RECALE, lower_clamp_rotation_failure, CLAMP_BLOCKED)
{
- return FSM_NEXT (CLAMP_READY_TO_RECALE,lower_clamp_rotation_failure);
}
FSM_TRANS_TIMEOUT (CLAMP_READY_TO_RECALE_2, TIMEOUT_OPEN_CLAMPS, CLAMP_TEMPO_RECALE)
{
IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE);
- return FSM_NEXT_TIMEOUT (CLAMP_READY_TO_RECALE_2);
}
FSM_TRANS_TIMEOUT (CLAMP_TEMPO_RECALE, TIMEOUT_OPEN_CLAMPS,CLAMP_END_RECALE)
{
move_needed2(CLAMP_1_DOWN,FAST_ROTATION,-1);
- return FSM_NEXT_TIMEOUT (CLAMP_TEMPO_RECALE);
}
FSM_TRANS (CLAMP_END_RECALE, lower_clamp_rotation_failure,CLAMP_BLOCKED)
{
- return FSM_NEXT (CLAMP_END_RECALE,lower_clamp_rotation_failure);
}
FSM_TRANS (CLAMP_END_RECALE, lower_clamp_rotation_success,CLAMP_TURN_HALF_WAY)
@@ -758,7 +719,6 @@ FSM_TRANS (CLAMP_END_RECALE, lower_clamp_rotation_success,CLAMP_TURN_HALF_WAY)
main_set_drop_coin_pos(ctx.current_pos + POS_TO_DROP + ctx.init_pos);
move_needed2(CLAMP_1_UP,FAST_ROTATION,1);
- return FSM_NEXT (CLAMP_END_RECALE, lower_clamp_rotation_success);
}
/*---------------------------------------------------------------------------------*/
/*Parts of the FSM that goes back to idle after receiving the stop approach signal */
@@ -774,44 +734,37 @@ FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS,stop_tree_approach,CLAMP_BOTTOM_CLAMP_BAC
{
move_needed2(CLAMP_1_DOWN,FAST_ROTATION,1);
}
- return FSM_NEXT(CLAMP_BOTTOM_CLAMP_HIDE_POS,stop_tree_approach);
}
FSM_TRANS (CLAMP_BOTTOM_CLAMP_BACK,lower_clamp_rotation_success,CLAMP_WAIT_BEFORE_IDLE)
{
- return FSM_NEXT(CLAMP_BOTTOM_CLAMP_BACK,lower_clamp_rotation_success);
}
FSM_TRANS (CLAMP_BOTTOM_CLAMP_BACK,lower_clamp_rotation_failure,CLAMP_BLOCKED)
{
clamp_blocked();
- return FSM_NEXT(CLAMP_BOTTOM_CLAMP_BACK,lower_clamp_rotation_failure);
}
FSM_TRANS (CLAMP_UNFOLD_UPPER_SET, stop_tree_approach,CLAMP_RELEASE_ASSERV)
{
ctx.stop_tree_approach = 1;
- return FSM_NEXT (CLAMP_UNFOLD_UPPER_SET, stop_tree_approach);
}
FSM_TRANS (CLAMP_BOTTOM_CLAMP_READY, stop_tree_approach,CLAMP_FINISH_BOTTOM_CLAMP_READY)
{
- return FSM_NEXT (CLAMP_BOTTOM_CLAMP_READY, stop_tree_approach);
}
FSM_TRANS (CLAMP_FINISH_BOTTOM_CLAMP_READY, lower_clamp_rotation_success,CLAMP_REARRANGE_CD)
{
- return FSM_NEXT (CLAMP_FINISH_BOTTOM_CLAMP_READY, lower_clamp_rotation_success);
}
FSM_TRANS (CLAMP_FINISH_BOTTOM_CLAMP_READY, lower_clamp_rotation_failure,CLAMP_BLOCKED)
{
clamp_blocked();
- return FSM_NEXT (CLAMP_FINISH_BOTTOM_CLAMP_READY, lower_clamp_rotation_failure);
}
@@ -824,7 +777,6 @@ FSM_TRANS (CLAMP_READY_TO_EMPTY_TREE, stop_tree_approach, CLAMP_REARRANGE_CD)
/*Closing upper & middle clamps.*/
IO_CLR (OUTPUT_UPPER_CLAMP_OPEN);
- return FSM_NEXT (CLAMP_READY_TO_EMPTY_TREE, stop_tree_approach);
}
/*---------------------------------------------------------------------------------*/
@@ -833,7 +785,6 @@ FSM_TRANS (CLAMP_READY_TO_EMPTY_TREE, stop_tree_approach, CLAMP_REARRANGE_CD)
/*---------------------------------------------------------------------------------*/
FSM_TRANS (CLAMP_BLOCKED_UPPER_SET_DOWN, upper_set_up, CLAMP_BLOCKED)
{
- return FSM_NEXT (CLAMP_BLOCKED_UPPER_SET_DOWN, upper_set_up);
}
FSM_TRANS_TIMEOUT (CLAMP_BLOCKED_UPPER_SET_DOWN, TIMEOUT_UPPER_SET_DOWN, CLAMP_BLOCKED)
@@ -841,7 +792,6 @@ FSM_TRANS_TIMEOUT (CLAMP_BLOCKED_UPPER_SET_DOWN, TIMEOUT_UPPER_SET_DOWN, CLAMP_B
/** folding upper set */
IO_CLR (OUTPUT_UPPER_CLAMP_DOWN);
IO_SET (OUTPUT_UPPER_CLAMP_UP);
- return FSM_NEXT_TIMEOUT (CLAMP_BLOCKED_UPPER_SET_DOWN);
}
/*---------------------------------------------------------------------------------*/
@@ -852,7 +802,7 @@ FSM_TRANS_TIMEOUT (CLAMP_BLOCKED_UPPER_SET_DOWN, TIMEOUT_UPPER_SET_DOWN, CLAMP_B
FSM_TRANS_TIMEOUT (CLAMP_BLOCKED,TIMEOUT_OPEN_CLAMPS,CLAMP_OPEN_BOTTOM_CLAMPS)
{
- fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
+ fsm_queue_post_event (FSM_EVENT (clamp_blocked));
/* Opening OR Closing the 2 clamps. */
if (ctx.cpt_blocked%2==0)
{
@@ -867,7 +817,6 @@ FSM_TRANS_TIMEOUT (CLAMP_BLOCKED,TIMEOUT_OPEN_CLAMPS,CLAMP_OPEN_BOTTOM_CLAMPS)
/*Setting upper clamp up*/
IO_CLR (OUTPUT_UPPER_CLAMP_DOWN);
IO_SET (OUTPUT_UPPER_CLAMP_UP);
- return FSM_NEXT_TIMEOUT (CLAMP_BLOCKED);
}
FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS, clamp_unblock, CLAMP_WAIT)
@@ -882,55 +831,41 @@ FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS, clamp_unblock, CLAMP_WAIT)
{
mimot_move_motor0_absolute (4000-pos, MEDIUM_ROTATION);
}
- return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, clamp_unblock);
}
FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS, tree_detected, CLAMP_OPEN_BOTTOM_CLAMPS)
{
- fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
- return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, tree_detected);
+ fsm_queue_post_event (FSM_EVENT (clamp_blocked));
}
FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS, robot_is_back, CLAMP_OPEN_BOTTOM_CLAMPS)
{
- fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
- return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, robot_is_back);
+ fsm_queue_post_event (FSM_EVENT (clamp_blocked));
}
FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS, clean_start, CLAMP_OPEN_BOTTOM_CLAMPS)
{
- fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
- return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, clean_start);
+ fsm_queue_post_event (FSM_EVENT ( clamp_blocked));
}
FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS,clean_catch, CLAMP_OPEN_BOTTOM_CLAMPS)
{
- fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
- return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, clean_catch);
+ fsm_queue_post_event (FSM_EVENT ( clamp_blocked));
}
FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS,clean_load, CLAMP_OPEN_BOTTOM_CLAMPS)
{
- fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
- return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, clean_load);
-}
-
-FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS,empty_tree, CLAMP_OPEN_BOTTOM_CLAMPS)
-{
- fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
- return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, empty_tree);
+ fsm_queue_post_event (FSM_EVENT ( clamp_blocked));
}
FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS,stop_tree_approach, CLAMP_OPEN_BOTTOM_CLAMPS)
{
- fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
- return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, stop_tree_approach);
+ fsm_queue_post_event (FSM_EVENT ( clamp_blocked));
}
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)
@@ -946,13 +881,11 @@ FSM_TRANS (CLAMP_WAIT,lower_clamp_rotation_success, CLAMP_TURN_BACKWARD)
IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE);
IO_SET (OUTPUT_LOWER_CLAMP_2_CLOSE);
}
- 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)
@@ -967,7 +900,6 @@ FSM_TRANS_TIMEOUT (CLAMP_TURN_BACKWARD,TIMEOUT_BLOCKED, CLAMP_TURN_FORWARD)
sens=-1;
}
move_needed2(CLAMP_1_DOWN,MEDIUM_ROTATION,sens);
- return FSM_NEXT_TIMEOUT (CLAMP_TURN_BACKWARD);
}
@@ -989,16 +921,16 @@ FSM_TRANS (CLAMP_TURN_FORWARD,lower_clamp_rotation_success,
move_needed2(HIDE_POS_TREE,FAST_ROTATION,1);
}
ctx.unblocking_upper_set = 0;
- return FSM_NEXT (CLAMP_TURN_FORWARD, lower_clamp_rotation_success, back_to_empty_tree);
+ return FSM_BRANCH (back_to_empty_tree);
}
else
{
ctx.cpt_blocked = 0;
if (ctx.upper_set_is_dead)
{
- fsm_queue_post_event (FSM_EVENT (AI, upper_set_is_dead));
+ fsm_queue_post_event (FSM_EVENT (upper_set_is_dead));
}
- return FSM_NEXT (CLAMP_TURN_FORWARD, lower_clamp_rotation_success,back_to_idle);
+ return FSM_BRANCH (back_to_idle);
}
}
@@ -1006,14 +938,12 @@ FSM_TRANS (CLAMP_TURN_FORWARD,lower_clamp_rotation_success,
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_BLOCKED)
{
ctx.cpt_blocked += 1;
- return FSM_NEXT (CLAMP_TURN_FORWARD, lower_clamp_rotation_failure);
}
/*---------------------------------------------------------------------------------*/
/* Parts of the FSM that takes care of the upper set when it's not going down */
@@ -1027,25 +957,23 @@ FSM_TRANS_TIMEOUT (CLAMP_UPPER_SET_BLOCKED_UP, TIMEOUT_OPEN_CLAMPS,
IO_SET (OUTPUT_UPPER_CLAMP_UP);
if (ctx.upper_set_blocked_cpt>=2)
{
- return FSM_NEXT_TIMEOUT (CLAMP_UPPER_SET_BLOCKED_UP, branch_upper_set_is_dead);
+ return FSM_BRANCH (branch_upper_set_is_dead);
}
else
{
- return FSM_NEXT_TIMEOUT (CLAMP_UPPER_SET_BLOCKED_UP,branch_try_to_deblock);
+ return FSM_BRANCH (branch_try_to_deblock);
}
}
FSM_TRANS_TIMEOUT (CLAMP_UPPER_SET_DEAD,TIMEOUT_OPEN_CLAMPS, CLAMP_BLOCKED)
{
ctx.upper_set_is_dead = 1;
- return FSM_NEXT_TIMEOUT (CLAMP_UPPER_SET_DEAD);
}
FSM_TRANS (CLAMP_UPPER_SET_FOLD_BACK,upper_set_up, CLAMP_TRY_ROTATION)
{
ctx.upper_set_is_dead = 1;
mimot_move_motor0_absolute (ctx.current_pos-4000, MEDIUM_ROTATION);
- return FSM_NEXT (CLAMP_UPPER_SET_FOLD_BACK,upper_set_up);
}
FSM_TRANS (CLAMP_TRY_ROTATION,lower_clamp_rotation_success,CLAMP_BOTTOM_CLAMP_HIDE_POS)
@@ -1059,13 +987,11 @@ FSM_TRANS (CLAMP_TRY_ROTATION,lower_clamp_rotation_success,CLAMP_BOTTOM_CLAMP_HI
{
move_needed2(HIDE_POS_TREE,FAST_ROTATION,1);
}
- return FSM_NEXT (CLAMP_TRY_ROTATION,lower_clamp_rotation_success);
}
FSM_TRANS (CLAMP_TRY_ROTATION,lower_clamp_rotation_failure,CLAMP_BLOCKED)
{
ctx.unblocking_upper_set = 1;
- return FSM_NEXT (CLAMP_TRY_ROTATION,lower_clamp_rotation_failure);
}
diff --git a/digital/io-hub/src/guybrush/main.c b/digital/io-hub/src/guybrush/main.c
index cfec1879..35f3a5c7 100644
--- a/digital/io-hub/src/guybrush/main.c
+++ b/digital/io-hub/src/guybrush/main.c
@@ -194,13 +194,13 @@ main_demo_events (void)
switch (tree_step)
{
case 0:
- clamp_request (FSM_EVENT (AI, tree_detected));
+ clamp_request (FSM_EVENT (tree_detected));
break;
case 1:
- clamp_request (FSM_EVENT (AI, empty_tree));
+ clamp_request (FSM_EVENT (empty_tree));
break;
case 2:
- clamp_request (FSM_EVENT (AI, robot_is_back));
+ clamp_request (FSM_EVENT (robot_is_back));
break;
}
tree_step = (tree_step + 1) % 3;
@@ -512,7 +512,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
case c ('j', 0):
/* Simulate jack insertion. */
- fsm_queue_post_event (FSM_EVENT (AI, jack_inserted));
+ fsm_queue_post_event (FSM_EVENT (jack_inserted));
break;
case c ('f', 0):
/* Enter FSM debug mode, then step once. */
@@ -520,36 +520,36 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
case c ('t',0):
/* Simulate tree detection. */
- fsm_queue_post_event (FSM_EVENT (AI, tree_detected));
+ fsm_queue_post_event (FSM_EVENT (tree_detected));
break;
case c ('s',0):
/* Simulate stop tree approach. */
- fsm_queue_post_event (FSM_EVENT (AI, stop_tree_approach));
+ fsm_queue_post_event (FSM_EVENT (stop_tree_approach));
break;
case c ('e',0):
/* Simulate the empty tree command. */
- fsm_queue_post_event (FSM_EVENT (AI, empty_tree));
+ fsm_queue_post_event (FSM_EVENT (empty_tree));
break;
case c ('r',0):
/* Simulate the robot_is_back command. */
- fsm_queue_post_event (FSM_EVENT (AI, robot_is_back));
+ fsm_queue_post_event (FSM_EVENT (robot_is_back));
break;
case c ('u',0):
/* Simulate the unblock command. */
- fsm_queue_post_event (FSM_EVENT (AI, clamp_unblock));
+ fsm_queue_post_event (FSM_EVENT (clamp_unblock));
break;
case c ('c', 1):
/* Simulate clean_start (00), clean_catch (01), clean_load (02). */
if (args[0] == 0)
- fsm_queue_post_event (FSM_EVENT (AI, clean_start));
+ fsm_queue_post_event (FSM_EVENT (clean_start));
else if (args[0] == 1)
- fsm_queue_post_event (FSM_EVENT (AI, clean_catch));
+ fsm_queue_post_event (FSM_EVENT (clean_catch));
else
- fsm_queue_post_event (FSM_EVENT (AI, clean_load));
+ fsm_queue_post_event (FSM_EVENT (clean_load));
break;
case c ('c',0):
/* Simulate the coin detected command. */
- fsm_queue_post_event (FSM_EVENT (AI, coin_detected));
+ fsm_queue_post_event (FSM_EVENT (coin_detected));
break;
case c ('m', 5):
/* Go to position.
diff --git a/digital/io-hub/src/guybrush/move.c b/digital/io-hub/src/guybrush/move.c
index 2e524463..22df5897 100644
--- a/digital/io-hub/src/guybrush/move.c
+++ b/digital/io-hub/src/guybrush/move.c
@@ -88,7 +88,7 @@ move_start (position_t position, uint8_t backward)
/* Reset try counter. */
move_data.try_again_counter = MOVE_TRY_AGAIN;
/* Start the FSM. */
- fsm_queue_post_event (FSM_EVENT (AI, move_start));
+ fsm_queue_post_event (FSM_EVENT (move_start));
}
void
@@ -103,14 +103,14 @@ move_start_noangle (vect_t position, uint8_t backward, int16_t shorten)
/* Reset try counter. */
move_data.try_again_counter = MOVE_TRY_AGAIN;
/* Start the FSM. */
- fsm_queue_post_event (FSM_EVENT (AI, move_start));
+ fsm_queue_post_event (FSM_EVENT (move_start));
}
void
move_stop (void)
{
/* Stop the FSM. */
- fsm_queue_post_event (FSM_EVENT (AI, move_stop));
+ fsm_queue_post_event (FSM_EVENT (move_stop));
}
void
@@ -323,14 +323,14 @@ FSM_TRANS (MOVE_IDLE, move_start,
if (next)
{
if (next == 2)
- return FSM_NEXT (MOVE_IDLE, move_start, path_found_rotate);
+ return FSM_BRANCH (path_found_rotate);
else
- return FSM_NEXT (MOVE_IDLE, move_start, path_found);
+ return FSM_BRANCH (path_found);
}
else
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_IDLE, move_start, no_path_found);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (no_path_found);
}
}
@@ -339,7 +339,6 @@ FSM_TRANS (MOVE_ROTATING,
MOVE_MOVING)
{
move_go ();
- return FSM_NEXT (MOVE_ROTATING, robot_move_success);
}
FSM_TRANS (MOVE_ROTATING,
@@ -347,20 +346,17 @@ FSM_TRANS (MOVE_ROTATING,
MOVE_MOVING)
{
move_go ();
- return FSM_NEXT (MOVE_ROTATING, robot_move_failure);
}
FSM_TRANS_TIMEOUT (MOVE_ROTATING, 1250,
MOVE_MOVING)
{
move_go ();
- return FSM_NEXT_TIMEOUT (MOVE_ROTATING);
}
FSM_TRANS (MOVE_ROTATING, move_stop, MOVE_IDLE)
{
asserv_stop_motor ();
- return FSM_NEXT (MOVE_ROTATING, move_stop);
}
FSM_TRANS (MOVE_MOVING, robot_move_success,
@@ -371,18 +367,18 @@ FSM_TRANS (MOVE_MOVING, robot_move_success,
{
if (move_data.final_move)
{
- fsm_queue_post_event (FSM_EVENT (AI, move_success));
- return FSM_NEXT (MOVE_MOVING, robot_move_success, done);
+ fsm_queue_post_event (FSM_EVENT (move_success));
+ return FSM_BRANCH (done);
}
else
{
uint8_t next = move_path_next ();
if (next == 2)
- return FSM_NEXT (MOVE_MOVING, robot_move_success, path_found_rotate);
+ return FSM_BRANCH (path_found_rotate);
else
- return FSM_NEXT (MOVE_MOVING, robot_move_success, path_found);
+ return FSM_BRANCH (path_found);
}
- //return FSM_NEXT (MOVE_MOVING, robot_move_success, no_path_found);
+ //return FSM_BRANCH (no_path_found);
}
/** Test if a point is ok to move back. */
@@ -436,21 +432,18 @@ FSM_TRANS (MOVE_MOVING,
MOVE_MOVING_BACKWARD_TO_TURN_FREELY)
{
move_moving_backward_to_turn_freely ();
- return FSM_NEXT (MOVE_MOVING, robot_move_failure);
}
FSM_TRANS_TIMEOUT (MOVE_MOVING, 2500,
MOVE_MOVING_BACKWARD_TO_TURN_FREELY)
{
move_moving_backward_to_turn_freely ();
- return FSM_NEXT_TIMEOUT (MOVE_MOVING);
}
FSM_TRANS (MOVE_MOVING, obstacle_in_front, MOVE_BRAKE)
{
move_data.final_move = 0;
asserv_stop_motor ();
- return FSM_NEXT (MOVE_MOVING, obstacle_in_front);
}
FSM_TRANS (MOVE_BRAKE, robot_move_success,
@@ -459,11 +452,11 @@ FSM_TRANS (MOVE_BRAKE, robot_move_success,
{
if (--move_data.try_again_counter == 0)
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_BRAKE, robot_move_success, tryout);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (tryout);
}
else
- return FSM_NEXT (MOVE_BRAKE, robot_move_success, tryagain);
+ return FSM_BRANCH (tryagain);
}
FSM_TRANS (MOVE_BRAKE, robot_move_failure,
@@ -472,17 +465,16 @@ FSM_TRANS (MOVE_BRAKE, robot_move_failure,
{
if (--move_data.try_again_counter == 0)
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_BRAKE, robot_move_failure, tryout);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (tryout);
}
else
- return FSM_NEXT (MOVE_BRAKE, robot_move_failure, tryagain);
+ return FSM_BRANCH (tryagain);
}
FSM_TRANS (MOVE_MOVING, move_stop, MOVE_IDLE)
{
asserv_stop_motor ();
- return FSM_NEXT (MOVE_MOVING, move_stop);
}
FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success,
@@ -493,8 +485,8 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success,
{
if (--move_data.try_again_counter == 0)
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, tryout);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (tryout);
}
else
{
@@ -502,14 +494,14 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success,
if (next)
{
if (next == 2)
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, path_found_rotate);
+ return FSM_BRANCH (path_found_rotate);
else
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, path_found);
+ return FSM_BRANCH (path_found);
}
else
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, no_path_found);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (no_path_found);
}
}
}
@@ -523,8 +515,8 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure,
{
if (--move_data.try_again_counter == 0)
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, tryout);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (tryout);
}
else
{
@@ -532,19 +524,19 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure,
if (next)
{
if (next == 2)
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, path_found_rotate);
+ return FSM_BRANCH (path_found_rotate);
else
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, path_found);
+ return FSM_BRANCH (path_found);
}
else
{
if (--move_data.try_again_counter == 0)
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, no_path_found_tryout);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (no_path_found_tryout);
}
else
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, no_path_found_tryagain);
+ return FSM_BRANCH (no_path_found_tryagain);
}
}
}
@@ -552,7 +544,6 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure,
FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop, MOVE_IDLE)
{
asserv_stop_motor ();
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop);
}
FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 250,
@@ -566,25 +557,24 @@ FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 250,
if (next)
{
if (next == 2)
- return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found_rotate);
+ return FSM_BRANCH (path_found_rotate);
else
- return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found);
+ return FSM_BRANCH (path_found);
}
else
{
/* Error, no new position, should we try again? */
if (--move_data.try_again_counter == 0)
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryout);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (no_path_found_tryout);
}
else
- return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryagain);
+ return FSM_BRANCH (no_path_found_tryagain);
}
}
FSM_TRANS (MOVE_WAIT_FOR_CLEAR_PATH, move_stop, MOVE_IDLE)
{
- return FSM_NEXT (MOVE_WAIT_FOR_CLEAR_PATH, move_stop);
}
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 3fa7ce5b..43ff7900 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -270,7 +270,6 @@ FSM_TRANS (TOP_START, init_actuators, TOP_INIT_DOOR)
main_demo = !IO_GET (CONTACT_STRAT);
IO_SET (OUTPUT_DOOR_OPEN);
IO_CLR (OUTPUT_DOOR_CLOSE);
- return FSM_NEXT (TOP_START, init_actuators);
}
FSM_TRANS (TOP_INIT_DOOR, init_done, TOP_INIT)
@@ -279,7 +278,6 @@ FSM_TRANS (TOP_INIT_DOOR, init_done, TOP_INIT)
IO_SET (OUTPUT_DOOR_CLOSE);
strat_init ();
strat_prepare ();
- return FSM_NEXT (TOP_INIT_DOOR, init_done);
}
FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION)
@@ -287,7 +285,6 @@ FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION)
main_usdist = 1;
beacon_on (1);
beacon_robot_nb (IO_GET (CONTACT_NB_ROBOTS) ? 1 : 2);
- return FSM_NEXT (TOP_INIT, init_start_round);
}
FSM_TRANS_TIMEOUT (TOP_DECISION, 1,
@@ -300,18 +297,18 @@ FSM_TRANS_TIMEOUT (TOP_DECISION, 1,
if (FSM_CAN_HANDLE (AI, clamp_unblock))
{
top_chaos_move ();
- return FSM_NEXT_TIMEOUT (TOP_DECISION, clamp_blocked);
+ return FSM_BRANCH (clamp_blocked);
}
switch (top_decision ())
{
default:
assert (0);
case STRAT_DECISION_TOTEM:
- return FSM_NEXT_TIMEOUT (TOP_DECISION, totem);
+ return FSM_BRANCH (totem);
case STRAT_DECISION_BOTTLE:
- return FSM_NEXT_TIMEOUT (TOP_DECISION, bottle);
+ return FSM_BRANCH (bottle);
case STRAT_DECISION_UNLOAD:
- return FSM_NEXT_TIMEOUT (TOP_DECISION, unload);
+ return FSM_BRANCH (unload);
}
}
@@ -320,19 +317,16 @@ FSM_TRANS (TOP_CLAMP_ERROR_MOVING_DECISION, robot_move_success,
{
radar_def_upper_clamp_moving (0);
FSM_HANDLE (AI, clamp_unblock);
- return FSM_NEXT (TOP_CLAMP_ERROR_MOVING_DECISION, robot_move_success);
}
FSM_TRANS (TOP_CLAMP_ERROR_MOVING_DECISION, robot_move_failure,
TOP_CLAMP_ERROR_DECISION)
{
FSM_HANDLE (AI, clamp_unblock);
- return FSM_NEXT (TOP_CLAMP_ERROR_MOVING_DECISION, robot_move_failure);
}
FSM_TRANS (TOP_CLAMP_ERROR_DECISION, clamps_ready, TOP_DECISION)
{
- return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, clamps_ready);
}
FSM_TRANS (TOP_CLAMP_ERROR_DECISION, clamp_blocked,
@@ -342,21 +336,20 @@ FSM_TRANS (TOP_CLAMP_ERROR_DECISION, clamp_blocked,
if (clamp_read_blocked_cpt () < TOP_CLAMP_BLOCKED_THRESHOLD)
{
FSM_HANDLE (AI, clamp_unblock);
- return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, clamp_blocked, unblock);
+ return FSM_BRANCH (unblock);
}
else
{
strat_clamp_dead ();
strat_bad_failure ();
FSM_HANDLE (AI, clamp_is_dead);
- return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, clamp_blocked, dead);
+ return FSM_BRANCH (dead);
}
}
FSM_TRANS (TOP_CLAMP_ERROR_DECISION, upper_set_is_dead, TOP_DECISION)
{
strat_upper_clamp_dead ();
- return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, upper_set_is_dead);
}
/** TOTEM */
@@ -364,8 +357,7 @@ FSM_TRANS (TOP_CLAMP_ERROR_DECISION, upper_set_is_dead, TOP_DECISION)
FSM_TRANS (TOP_TOTEM_GOING, move_success, TOP_TOTEM_CLEAN_STARTING)
{
radar_def_upper_clamp_moving (1);
- clamp_request (FSM_EVENT (AI, clean_start));
- return FSM_NEXT (TOP_TOTEM_GOING, move_success);
+ clamp_request (FSM_EVENT (clean_start));
}
FSM_TRANS (TOP_TOTEM_CLEAN_STARTING, clamps_ready, TOP_TOTEM_CLEAN_APPROACHING)
@@ -374,14 +366,12 @@ FSM_TRANS (TOP_TOTEM_CLEAN_STARTING, clamps_ready, TOP_TOTEM_CLEAN_APPROACHING)
top.totem_distance -= move;
asserv_set_speed (BOT_SPEED_APPROACH);
asserv_move_linearly (move);
- return FSM_NEXT (TOP_TOTEM_CLEAN_STARTING, clamps_ready);
}
FSM_TRANS (TOP_TOTEM_CLEAN_APPROACHING, robot_move_success,
TOP_TOTEM_CLEAN_CATCH_WAITING)
{
FSM_HANDLE (AI, clean_catch);
- return FSM_NEXT (TOP_TOTEM_CLEAN_APPROACHING, robot_move_success);
}
FSM_TRANS (TOP_TOTEM_CLEAN_APPROACHING, robot_move_failure,
@@ -389,7 +379,6 @@ FSM_TRANS (TOP_TOTEM_CLEAN_APPROACHING, robot_move_failure,
{
/* Must be blocked on a CD, catch and continue. */
FSM_HANDLE (AI, clean_catch);
- return FSM_NEXT (TOP_TOTEM_CLEAN_APPROACHING, robot_move_failure);
}
FSM_TRANS (TOP_TOTEM_CLEAN_CATCH_WAITING, clamps_ready,
@@ -398,27 +387,23 @@ FSM_TRANS (TOP_TOTEM_CLEAN_CATCH_WAITING, clamps_ready,
int16_t move = top.totem_distance - BOT_SIZE_LOADING_FRONT - 30;
top.totem_distance -= move;
asserv_move_linearly (move);
- return FSM_NEXT (TOP_TOTEM_CLEAN_CATCH_WAITING, clamps_ready);
}
FSM_TRANS (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_success,
TOP_TOTEM_CLEAN_LOADING)
{
FSM_HANDLE (AI, clean_load);
- return FSM_NEXT (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_success);
}
FSM_TRANS (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_failure,
TOP_TOTEM_CLEAN_LOADING)
{
FSM_HANDLE (AI, clean_load);
- return FSM_NEXT (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_failure);
}
FSM_TRANS (TOP_TOTEM_CLEAN_LOADING, clamps_ready, TOP_TOTEM_CLAMP_DOWNING)
{
- clamp_request (FSM_EVENT (AI, tree_detected));
- return FSM_NEXT (TOP_TOTEM_CLEAN_LOADING, clamps_ready);
+ clamp_request (FSM_EVENT (tree_detected));
}
FSM_TRANS (TOP_TOTEM_CLAMP_DOWNING, clamps_ready, TOP_TOTEM_APPROACHING)
@@ -426,38 +411,32 @@ FSM_TRANS (TOP_TOTEM_CLAMP_DOWNING, clamps_ready, TOP_TOTEM_APPROACHING)
int16_t move = top.totem_distance - BOT_SIZE_FRONT - 30;
top.totem_distance -= move;
asserv_move_linearly (move);
- 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_EMPTYING)
{
asserv_stop_motor ();
FSM_HANDLE (AI, empty_tree);
- return FSM_NEXT (TOP_TOTEM_PUSHING, robot_move_success);
}
FSM_TRANS (TOP_TOTEM_EMPTYING, clamps_ready, TOP_TOTEM_GOING_BACK)
{
move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
- return FSM_NEXT (TOP_TOTEM_EMPTYING, clamps_ready);
}
FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success, TOP_TOTEM_CLAMP_UPPING)
{
strat_success ();
FSM_HANDLE (AI, robot_is_back);
- return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_success);
}
FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamps_ready, TOP_DECISION)
{
- return FSM_NEXT (TOP_TOTEM_CLAMP_UPPING, clamps_ready);
}
/** TOTEM failures. */
@@ -465,7 +444,6 @@ FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamps_ready, TOP_DECISION)
FSM_TRANS (TOP_TOTEM_GOING, move_failure, TOP_DECISION)
{
strat_failure ();
- return FSM_NEXT (TOP_TOTEM_GOING, move_failure);
}
FSM_TRANS (TOP_TOTEM_CLEAN_STARTING, clamp_blocked,
@@ -473,13 +451,11 @@ FSM_TRANS (TOP_TOTEM_CLEAN_STARTING, clamp_blocked,
{
strat_failure ();
top_chaos_move ();
- return FSM_NEXT (TOP_TOTEM_CLEAN_STARTING, clamp_blocked);
}
FSM_TRANS_TIMEOUT (TOP_TOTEM_CLEAN_STARTING, 5 * 250, TOP_DECISION)
{
strat_bad_failure ();
- return FSM_NEXT_TIMEOUT (TOP_TOTEM_CLEAN_STARTING);
}
FSM_TRANS (TOP_TOTEM_CLEAN_CATCH_WAITING, clamp_blocked,
@@ -487,7 +463,6 @@ FSM_TRANS (TOP_TOTEM_CLEAN_CATCH_WAITING, clamp_blocked,
{
strat_failure ();
move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
- return FSM_NEXT (TOP_TOTEM_CLEAN_CATCH_WAITING, clamp_blocked);
}
FSM_TRANS (TOP_TOTEM_CLEAN_LOADING, clamp_blocked,
@@ -495,7 +470,6 @@ FSM_TRANS (TOP_TOTEM_CLEAN_LOADING, clamp_blocked,
{
strat_failure ();
top_chaos_move ();
- return FSM_NEXT (TOP_TOTEM_CLEAN_LOADING, clamp_blocked);
}
FSM_TRANS (TOP_TOTEM_CLAMP_DOWNING, clamp_blocked,
@@ -503,13 +477,11 @@ FSM_TRANS (TOP_TOTEM_CLAMP_DOWNING, clamp_blocked,
{
strat_failure ();
top_chaos_move ();
- return FSM_NEXT (TOP_TOTEM_CLAMP_DOWNING, clamp_blocked);
}
FSM_TRANS_TIMEOUT (TOP_TOTEM_CLAMP_DOWNING, 5 * 250, TOP_DECISION)
{
strat_bad_failure ();
- return FSM_NEXT_TIMEOUT (TOP_TOTEM_CLAMP_DOWNING);
}
FSM_TRANS (TOP_TOTEM_APPROACHING, robot_move_failure,
@@ -517,7 +489,6 @@ FSM_TRANS (TOP_TOTEM_APPROACHING, robot_move_failure,
{
strat_giveup ();
move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
- return FSM_NEXT (TOP_TOTEM_APPROACHING, robot_move_failure);
}
FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_failure,
@@ -525,7 +496,6 @@ FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_failure,
{
strat_giveup ();
move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
- return FSM_NEXT (TOP_TOTEM_PUSHING, robot_move_failure);
}
FSM_TRANS (TOP_TOTEM_EMPTYING, clamp_blocked,
@@ -533,14 +503,12 @@ FSM_TRANS (TOP_TOTEM_EMPTYING, clamp_blocked,
{
strat_giveup ();
move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
- return FSM_NEXT (TOP_TOTEM_EMPTYING, clamp_blocked);
}
FSM_TRANS (TOP_TOTEM_GOING_BACK, move_failure, TOP_TOTEM_ERROR_RELEASE)
{
strat_giveup ();
FSM_HANDLE (AI, stop_tree_approach);
- return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_failure);
}
FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamp_blocked,
@@ -548,39 +516,33 @@ FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamp_blocked,
{
strat_giveup ();
top_chaos_move ();
- return FSM_NEXT (TOP_TOTEM_CLAMP_UPPING, clamp_blocked);
}
FSM_TRANS (TOP_TOTEM_ERROR_RELEASE, clamps_ready, TOP_TOTEM_ERROR_GOING_BACK)
{
move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
- return FSM_NEXT (TOP_TOTEM_ERROR_RELEASE, clamps_ready);
}
FSM_TRANS (TOP_TOTEM_ERROR_GOING_BACK, move_success, TOP_TOTEM_CLAMP_UPPING)
{
FSM_HANDLE (AI, stop_tree_approach);
- return FSM_NEXT (TOP_TOTEM_ERROR_GOING_BACK, move_success);
}
FSM_TRANS (TOP_TOTEM_ERROR_GOING_BACK, move_failure, TOP_TOTEM_CLAMP_UPPING)
{
FSM_HANDLE (AI, stop_tree_approach);
- return FSM_NEXT (TOP_TOTEM_ERROR_GOING_BACK, move_failure);
}
FSM_TRANS (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_success,
TOP_CLAMP_ERROR_MOVING_DECISION)
{
top_chaos_move ();
- return FSM_NEXT (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_success);
}
FSM_TRANS (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_failure,
TOP_CLAMP_ERROR_MOVING_DECISION)
{
top_chaos_move ();
- return FSM_NEXT (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_failure);
}
/** BOTTLE */
@@ -590,13 +552,11 @@ FSM_TRANS (TOP_BOTTLE_GOING, move_success, TOP_BOTTLE_APPROACHING)
int16_t move = top.decision_pos.y - BOT_SIZE_BACK - 22 - 30;
asserv_set_speed (BOT_SPEED_APPROACH);
asserv_move_linearly (-move);
- return FSM_NEXT (TOP_BOTTLE_GOING, move_success);
}
FSM_TRANS (TOP_BOTTLE_APPROACHING, robot_move_success, TOP_BOTTLE_PUSHING)
{
asserv_push_the_wall (ASSERV_BACKWARD, -1, -1, -1);
- return FSM_NEXT (TOP_BOTTLE_APPROACHING, robot_move_success);
}
FSM_TRANS (TOP_BOTTLE_PUSHING, robot_move_success, TOP_BOTTLE_GOING_BACK)
@@ -604,7 +564,6 @@ FSM_TRANS (TOP_BOTTLE_PUSHING, robot_move_success, TOP_BOTTLE_GOING_BACK)
asserv_stop_motor ();
strat_success ();
move_start_noangle (top.decision_pos, 0, 0);
- return FSM_NEXT (TOP_BOTTLE_PUSHING, robot_move_success);
}
FSM_TRANS (TOP_BOTTLE_PUSHING, robot_move_failure, TOP_BOTTLE_GOING_BACK)
@@ -613,18 +572,15 @@ FSM_TRANS (TOP_BOTTLE_PUSHING, robot_move_failure, TOP_BOTTLE_GOING_BACK)
asserv_stop_motor ();
strat_success ();
move_start_noangle (top.decision_pos, 0, 0);
- return FSM_NEXT (TOP_BOTTLE_PUSHING, robot_move_failure);
}
FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_success, TOP_DECISION)
{
- return FSM_NEXT (TOP_BOTTLE_GOING_BACK, move_success);
}
FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_failure, TOP_DECISION)
{
/* Ignore and continue. */
- return FSM_NEXT (TOP_BOTTLE_GOING_BACK, move_failure);
}
/** BOTTLE failures. */
@@ -632,7 +588,6 @@ FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_failure, TOP_DECISION)
FSM_TRANS (TOP_BOTTLE_GOING, move_failure, TOP_DECISION)
{
strat_failure ();
- return FSM_NEXT (TOP_BOTTLE_GOING, move_failure);
}
FSM_TRANS (TOP_BOTTLE_APPROACHING, robot_move_failure,
@@ -645,14 +600,12 @@ FSM_TRANS (TOP_BOTTLE_APPROACHING, robot_move_failure,
if (robot_pos.v.y < BOT_SIZE_BACK + 100)
{
asserv_push_the_wall (ASSERV_BACKWARD, -1, -1, -1);
- return FSM_NEXT (TOP_BOTTLE_APPROACHING, robot_move_failure,
- try_anyway);
+ return FSM_BRANCH (try_anyway);
}
else
{
strat_failure ();
- return FSM_NEXT (TOP_BOTTLE_APPROACHING, robot_move_failure,
- decision);
+ return FSM_BRANCH (decision);
}
}
@@ -674,30 +627,27 @@ FSM_TRANS (TOP_UNLOAD_GOING, move_success,
if (top.decision_pos.y < PG_LENGTH - PG_CAPTAIN_ROOM_LENGTH_MM)
{
asserv_move_linearly (-100);
- return FSM_NEXT (TOP_UNLOAD_GOING, move_success, hold);
+ return FSM_BRANCH (hold);
}
else
{
asserv_move_linearly (-50);
- return FSM_NEXT (TOP_UNLOAD_GOING, move_success, captain);
+ return FSM_BRANCH (captain);
}
}
FSM_TRANS (TOP_UNLOAD_GOING_BACK, robot_move_success, TOP_UNLOADING)
{
top_do_unload ();
- return FSM_NEXT (TOP_UNLOAD_GOING_BACK, robot_move_success);
}
FSM_TRANS (TOP_UNLOAD_GOING_BACK, robot_move_failure, TOP_UNLOADING)
{
top_do_unload ();
- return FSM_NEXT (TOP_UNLOAD_GOING_BACK, robot_move_failure);
}
FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250, TOP_DECISION)
{
- return FSM_NEXT_TIMEOUT (TOP_UNLOADING);
}
/** UNLOAD failures. */
@@ -705,6 +655,5 @@ FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250, TOP_DECISION)
FSM_TRANS (TOP_UNLOAD_GOING, move_failure, TOP_DECISION)
{
strat_failure ();
- return FSM_NEXT (TOP_UNLOAD_GOING, move_failure);
}