From 73bbc9e206772e928878126187789448ab3d0d47 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 20 May 2009 16:04:33 +0200 Subject: * digital/io/src: - add direction support in goto command, - move FSM use the angle for the end of the movement. --- digital/io/src/asserv.c | 4 ++-- digital/io/src/asserv.h | 3 ++- digital/io/src/init_cb.c | 2 +- digital/io/src/move.c | 16 +++++--------- digital/io/src/move.h | 13 +++++++----- digital/io/src/move_cb.c | 55 ++++++++++++++++++++++++------------------------ 6 files changed, 45 insertions(+), 48 deletions(-) (limited to 'digital') diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index 3a22d0bc..062ca13a 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -460,7 +460,7 @@ asserv_goto_angle (int16_t angle) /* Go to an absolute position and then an absolute angle. */ void -asserv_goto_xya (uint32_t x, uint32_t y, int16_t a) +asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward) { x = fixed_mul_f824 (x, asserv_scale_inv); y = fixed_mul_f824 (y, asserv_scale_inv); @@ -476,7 +476,7 @@ asserv_goto_xya (uint32_t x, uint32_t y, int16_t a) asserv_twi_buffer_param[6] = v16_to_v8 (a, 1); asserv_twi_buffer_param[7] = v16_to_v8 (a, 0); /* No backward. */ - asserv_twi_buffer_param[8] = 0; + asserv_twi_buffer_param[8] = backward; /* Send the got to an absolute position and them absolute angle command to * the asserv board */ asserv_twi_send_command ('X', 9); diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h index 7e2ba988..82f405c0 100644 --- a/digital/io/src/asserv.h +++ b/digital/io/src/asserv.h @@ -226,9 +226,10 @@ asserv_goto_angle (int16_t angle); * @param x the absolute position on the X axis. * @param y the absolute position on the Y axis. * @param a the absolute angle. + * @param backward 0 no backward, 1 backward possible, 2 backward compulsary. */ void -asserv_goto_xya (uint32_t x, uint32_t y, int16_t a); +asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward); /** * Go to the wall (moving backward). diff --git a/digital/io/src/init_cb.c b/digital/io/src/init_cb.c index 063314a3..b74937b2 100644 --- a/digital/io/src/init_cb.c +++ b/digital/io/src/init_cb.c @@ -211,7 +211,7 @@ init__GO_AWAY_FROM_THE_WALL_AGAIN__bot_move_succeed (void) - BOT_WIDTH / 2 - 50), PG_LENGTH - PG_START_ZONE_LENGTH + BOT_LENGTH / 2 + 50, - PG_A_VALUE_COMPUTING (0 * BOT_ANGLE_DEGREE)); + PG_A_VALUE_COMPUTING (0 * BOT_ANGLE_DEGREE), 0); return init_next (GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed); } diff --git a/digital/io/src/move.c b/digital/io/src/move.c index c30e5ace..7acc6c8b 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -34,20 +34,14 @@ */ struct move_data_t move_data; -/** - * Go to a position with the start FSM. - * @param position_x the X position. - * @param position_y the Y position. - * @param backward_movement_allowed do we allow backward movement? - */ void -move_start (uint16_t position_x, uint16_t position_y, uint8_t - backward_movement_allowed) +move_start (asserv_position_t position, uint8_t backward) { /* Set parameters. */ - move_data.final.x = position_x; - move_data.final.y = position_y; - move_data.backward_movement_allowed = backward_movement_allowed; + move_data.final.x = position.x; + move_data.final.y = position.y; + move_data.final.a = position.a; + move_data.backward_movement_allowed = backward; /* XXX */ main_always_stop_for_obstacle = 1; /* Reset move FSM flags */ diff --git a/digital/io/src/move.h b/digital/io/src/move.h index 1b1621af..40fadf30 100644 --- a/digital/io/src/move.h +++ b/digital/io/src/move.h @@ -25,6 +25,8 @@ * * }}} */ +#include "asserv.h" + /** * A position. */ @@ -34,6 +36,8 @@ typedef struct move_position_t uint16_t x; /** Y position. */ uint16_t y; + /** A angle. */ + uint16_t a; } move_position_t; /** @@ -60,12 +64,11 @@ extern struct move_data_t move_data; /** * Go to a position with the start FSM. - * @param position_x the X position. - * @param position_y the Y position. - * @param backward_movement_allowed do we allow backward movement? + * @param position the destination position. + * @param backward_movement_allowed 0, no backward, 1 backward is allowed, 2 + * backward is compulsary. */ void -move_start (uint16_t position_x, uint16_t position_y, uint8_t - backward_movement_allowed); +move_start (asserv_position_t position, uint8_t backward); #endif /* move_h */ diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index a5622369..98f609b6 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -243,10 +243,10 @@ move_after_moving_backward (void) if (move_get_next_position (&move_data.intermediate)) { /* Go to the next position */ - 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); + asserv_goto_xya (move_data.intermediate.x, + move_data.intermediate.y, + move_data.intermediate.a, + move_data.backward_movement_allowed); return 1; } else @@ -273,10 +273,9 @@ move__WAIT_FOR_CLEAR_PATH__wait_finished (void) if (!sharp_path_obstrued (1)) { /* Try to go to the final position */ - 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); + asserv_goto_xya (move_data.final.x, move_data.final.y, + move_data.final.a, + move_data.backward_movement_allowed); return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, no_obstacle); } else @@ -289,10 +288,10 @@ move__WAIT_FOR_CLEAR_PATH__wait_finished (void) /* Ignore sharps */ main_sharp_ignore_event = MOVE_MAIN_IGNORE_SHARP_EVENT; /* Go to the next intermediate position */ - 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); + asserv_goto_xya (move_data.intermediate.x, + move_data.intermediate.y, + move_data.intermediate.a, + move_data.backward_movement_allowed); return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle_and_intermediate_path_found); } else @@ -317,10 +316,10 @@ fsm_branch_t move__IDLE__start (void) { /* Go to the destination position */ - 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); + asserv_goto_xya (move_data.final.x, + move_data.final.y, + move_data.final.a, + move_data.backward_movement_allowed); return move_next (IDLE, start); } @@ -351,10 +350,10 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_succeed (void) if (move_get_next_position (&move_data.intermediate)) { /* Go to the next intermediate position */ - 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); + asserv_goto_xya (move_data.intermediate.x, + move_data.intermediate.y, + move_data.intermediate.a, + move_data.backward_movement_allowed); return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, position_intermediary); } else @@ -392,10 +391,10 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_obstacle (void) if (move_get_next_position (&move_data.intermediate)) { /* Go to the next intermediate position */ - 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); + asserv_goto_xya (move_data.intermediate.x, + move_data.intermediate.y, + move_data.intermediate.a, + move_data.backward_movement_allowed); return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_obstacle, intermediate_path_found); } else @@ -539,10 +538,10 @@ move__MOVING_TO_FINAL_POSITION__bot_move_obstacle (void) if (move_get_next_position (&move_data.intermediate)) { /* Go to the next intermediate position */ - 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); + asserv_goto_xya (move_data.intermediate.x, + move_data.intermediate.y, + move_data.intermediate.a, + move_data.backward_movement_allowed); return move_next_branch (MOVING_TO_FINAL_POSITION, bot_move_obstacle, intermediate_path_found); } else -- cgit v1.2.3