summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/guybrush/move.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub/src/guybrush/move.c')
-rw-r--r--digital/io-hub/src/guybrush/move.c82
1 files changed, 36 insertions, 46 deletions
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);
}