From a48c26a13614dab1af11d3a6b46a6dc0921e2756 Mon Sep 17 00:00:00 2001 From: NĂ©lio Laranjeiro Date: Sun, 27 Apr 2008 11:35:03 +0200 Subject: move.fsm: Update the fsm to use the path finder. * update the events in the main.c file. * update the move_cb.c * added some constants. --- digital/io/src/move.c | 76 +-------------------------------------------------- 1 file changed, 1 insertion(+), 75 deletions(-) (limited to 'digital/io/src/move.c') diff --git a/digital/io/src/move.c b/digital/io/src/move.c index e360dfa1..53709d07 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -27,14 +27,6 @@ #include "move.h" #include "fsm.h" -#define MOVE_BORDER_END 500 - -#define MOVE_ANGLE_0 0x0 -#define MOVE_ANGLE_90 0x4000 -#define MOVE_ANGLE_180 0x8000 -#define MOVE_ANGLE_270 0xC000 - - struct move_data_t move_data; /** Start a move FSM. */ @@ -44,75 +36,9 @@ move_start (uint32_t position_x, uint32_t position_y) /* Set parameters. */ move_data.position_x = position_x; move_data.position_y = position_y; + move_data.nb_obstacle = 0; /* Start the FSM. */ fsm_init (&move_fsm); fsm_handle_event (&move_fsm, MOVE_EVENT_start); } -/** Verify if the position desired is in the table use when the robot tries to - * reach a point and a obstacle is in front of it. - * \param pos the robot's current position on the table. - * \param new_pos the position desired by the user. - * \return true if the it can reach the position. - */ -uint8_t -move_can_go_on_left_or_right (asserv_position_t current_pos, - asserv_position_t new_pos) -{ - // Go on right. - if (new_pos.x > current_pos.x) - { - if (new_pos.x - current_pos.x < 3000 - MOVE_BORDER_END) - return 0x1; - else - return 0x0; - } - // go on left. - else - { - if (current_pos.x - new_pos.x < MOVE_BORDER_END) - return 0x1; - else - return 0x0; - } -} - -/** Go to the right. - */ -void -move_go_to_right (void) -{ - asserv_position_t pos; - asserv_position_t new_pos; - - asserv_get_position (&pos); - new_pos.x = pos.x - MOVE_BORDER_LEVEL; - new_pos.y = pos.y; - new_pos.a = pos.a; - - if (move_can_go_on_left_or_right (pos, new_pos)) - { - asserv_goto (new_pos.x, new_pos.y); - } -} - -/** Go to the left. - */ -void -move_go_to_left (void) -{ - asserv_position_t pos; - asserv_position_t new_pos; - - asserv_get_position (&pos); - new_pos.x = pos.x + MOVE_BORDER_LEVEL; - new_pos.y = pos.y; - new_pos.a = pos.a; - - if (move_can_go_on_left_or_right (pos, new_pos)) - { - asserv_goto (new_pos.x, new_pos.y); - } -} - - -- cgit v1.2.3