From 2183136d23f1b2a3eb5ee89c5902e16308036f45 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 30 Apr 2008 21:09:18 +0200 Subject: * digital/io/src - integrate backward movement support in the move FSM. --- digital/io/src/move_cb.c | 28 +++++++++++++++++++++++----- 1 file changed, 23 insertions(+), 5 deletions(-) (limited to 'digital/io/src/move_cb.c') diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index a92135ea..7254a504 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -109,6 +109,9 @@ move_compute_obstacle_position (asserv_position_t cur, { /* Convert the angle */ uint32_t angle = cur.a; + /* Change angle when going backward */ + if (move_data.backward_movement_allowed) + angle += 0x8000; angle = angle << 8; /* X */ obstacle->x = cur.x + fixed_mul_f824 (fixed_cos_f824 (angle), @@ -142,7 +145,10 @@ move_after_moving_backward (void) /* Give the current position of the bot to the path module */ move_get_next_position (&move_data.intermediate); /* Go to the next position */ - asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + if (move_data.backward_movement_allowed) + asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y); + else + asserv_goto (move_data.intermediate.x, move_data.intermediate.y); } /* @@ -154,7 +160,10 @@ fsm_branch_t move__IDLE__start (void) { /* Go to the destination position */ - asserv_goto (move_data.final.x, move_data.final.y); + if (move_data.backward_movement_allowed) + asserv_goto_back (move_data.final.x, move_data.final.y); + else + asserv_goto (move_data.final.x, move_data.final.y); return move_next (IDLE, start); } @@ -180,7 +189,10 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_succeed (void) /* Get next position */ move_get_next_position (&move_data.intermediate); /* Go to the next intermediate position */ - asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + if (move_data.backward_movement_allowed) + asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y); + else + asserv_goto (move_data.intermediate.x, move_data.intermediate.y); return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, position_intermediary); } } @@ -200,7 +212,10 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_obstacle (void) /* Get next position */ move_get_next_position (&move_data.intermediate); /* Go to the next intermediate position */ - asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + if (move_data.backward_movement_allowed) + asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y); + else + asserv_goto (move_data.intermediate.x, move_data.intermediate.y); return move_next (MOVING_TO_INTERMEDIATE_POSITION, bot_move_obstacle); } @@ -296,7 +311,10 @@ move__MOVING_TO_FINAL_POSITION__bot_move_obstacle (void) /* Get next position */ move_get_next_position (&move_data.intermediate); /* Go to the next intermediate position */ - asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + if (move_data.backward_movement_allowed) + asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y); + else + asserv_goto (move_data.intermediate.x, move_data.intermediate.y); return move_next (MOVING_TO_FINAL_POSITION, bot_move_obstacle); } -- cgit v1.2.3