From b9d646e41511eb5443faad66b8fc1c7323b2e738 Mon Sep 17 00:00:00 2001 From: NĂ©lio Laranjeiro Date: Tue, 8 Apr 2008 00:02:53 +0200 Subject: Tested the move FSM machine. --- digital/io/src/move.c | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) (limited to 'digital/io/src/move.c') diff --git a/digital/io/src/move.c b/digital/io/src/move.c index 211c53ac..b4155589 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -23,9 +23,18 @@ * * }}} */ #include "common.h" +#include "asserv.h" #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. */ @@ -40,3 +49,31 @@ move_start (uint32_t position_x, uint32_t position_y) fsm_handle_event (&move_fsm, MOVE_EVENT_ok); } +/** 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; + } +} + -- cgit v1.2.3