From f8d057385f4592e43601e20cba2b7ea60f57922c Mon Sep 17 00:00:00 2001 From: Jerome Jutteau Date: Fri, 8 Mar 2013 17:38:04 +0100 Subject: digital/io-hub/src/guybrush: update FSM --- digital/io-hub/src/guybrush/Makefile | 2 +- digital/io-hub/src/guybrush/bottom_clamp.c | 140 +++++++---------------------- digital/io-hub/src/guybrush/main.c | 26 +++--- digital/io-hub/src/guybrush/move.c | 82 ++++++++--------- digital/io-hub/src/guybrush/top.c | 75 +++------------- 5 files changed, 95 insertions(+), 230 deletions(-) (limited to 'digital/io-hub/src') 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); } -- cgit v1.2.3