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 ++++++++++++++++ digital/io/src/move.fsm | 14 +++--- digital/io/src/move.h | 12 ++++++ digital/io/src/move_cb.c | 95 +++++++++++++++++++++++++++-------------- digital/io/src/test/move/main.c | 59 +++++++++++++++++-------- 5 files changed, 159 insertions(+), 58 deletions(-) (limited to 'digital') 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; + } +} + diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index da059071..cbe38ba9 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -11,7 +11,7 @@ States: Events: ok reached - near_border + failed_or_blocked_or_near_border failed_or_blocked START: @@ -21,21 +21,19 @@ START: DESIRED_POSITION: reached -> END The position provided by the user has been reached, the FSM can stop. - failed_or_blocked -> MOVE_ON_LEFT + failed_or_blocked: near_right_border -> MOVE_ON_LEFT The robot has failed to reach the position. It shall try another position before trying to reach this one again. It shall go to the on the left only if the left border is the farest one. + failed_or_blocked: near_left_border -> MOVE_ON_RIGHT + Same process as the previous one but on the right. MOVE_ON_LEFT: reached -> DESIRED_POSITION The position has been reached. It will now try to reach the position provided by the user. - failed_or_blocked -> MOVE_ON_LEFT + failed_or_blocked_or_near_border -> MOVE_ON_RIGHT The position is fail again, it will try to reach another one. - near_border -> MOVE_ON_RIGHT - The robot is now too near of the border. MOVE_ON_RIGHT: reached -> DESIRED_POSITION The position has been reached. It will now try to reach the position provided by the user. - failed_or_blocked -> MOVE_ON_RIGHT + failed_or_blocked_or_near_border -> MOVE_ON_LEFT The position is fail again, it will try to reach another one. - near_border -> MOVE_ON_LEFT - The robot is now too near of the border. diff --git a/digital/io/src/move.h b/digital/io/src/move.h index a416043d..99997bc3 100644 --- a/digital/io/src/move.h +++ b/digital/io/src/move.h @@ -25,6 +25,8 @@ * * }}} */ +#include "asserv.h" + /** move FSM associated data. */ struct move_data_t { @@ -42,4 +44,14 @@ extern struct move_data_t move_data; void move_start (uint32_t position_x, uint32_t position_y); +/** 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 computed to go to. + * \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); + #endif /* move_h */ diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index 05217c02..7a055e10 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -11,6 +11,10 @@ #include "move.h" #include "asserv.h" +/* if the left border is under 500 mm of the actual position do not go there. */ +#define MOVE_BORDER_LEVEL 200 + + /* * START =ok=> * => DESIRED_POSITION @@ -19,18 +23,39 @@ fsm_branch_t move__START__ok (void) { + asserv_goto (move_data.position_x, move_data.position_y); return move_next (START, ok); } /* * DESIRED_POSITION =failed_or_blocked=> - * => MOVE_ON_LEFT + * near_left_border => MOVE_ON_RIGHT + * Same process as the previous one but on the right. + * near_right_border => MOVE_ON_LEFT * The robot has failed to reach the position. It shall try another position before trying to reach this one again. It shall go to the on the left only if the left border is the farest one. */ fsm_branch_t move__DESIRED_POSITION__failed_or_blocked (void) { - return move_next (DESIRED_POSITION, failed_or_blocked); + asserv_position_t pos; + asserv_position_t new_pos; + + asserv_get_position (&pos); + new_pos = pos; + new_pos.x += MOVE_BORDER_LEVEL ; + + if (move_can_go_on_left_or_right (pos, new_pos)) + { + asserv_goto (new_pos.x, new_pos.y); + return move_next_branch (DESIRED_POSITION, failed_or_blocked, near_left_border); + } + else + { + new_pos.x = pos.x + MOVE_BORDER_LEVEL; + // replace this by the correct function. + asserv_goto (new_pos.x, new_pos.y); + return move_next_branch (DESIRED_POSITION, failed_or_blocked, near_right_border); + } } /* @@ -45,14 +70,27 @@ move__DESIRED_POSITION__reached (void) } /* - * MOVE_ON_RIGHT =failed_or_blocked=> - * => MOVE_ON_RIGHT + * MOVE_ON_RIGHT =failed_or_blocked_or_near_border=> + * => MOVE_ON_LEFT * The position is fail again, it will try to reach another one. */ fsm_branch_t -move__MOVE_ON_RIGHT__failed_or_blocked (void) +move__MOVE_ON_RIGHT__failed_or_blocked_or_near_border (void) { - return move_next (MOVE_ON_RIGHT, failed_or_blocked); + 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); + } + + return move_next (MOVE_ON_RIGHT, failed_or_blocked_or_near_border); } /* @@ -63,29 +101,32 @@ move__MOVE_ON_RIGHT__failed_or_blocked (void) fsm_branch_t move__MOVE_ON_RIGHT__reached (void) { + asserv_goto (move_data.position_x, move_data.position_y); return move_next (MOVE_ON_RIGHT, reached); } /* - * MOVE_ON_RIGHT =near_border=> - * => MOVE_ON_LEFT - * The robot is now too near of the border. - */ -fsm_branch_t -move__MOVE_ON_RIGHT__near_border (void) -{ - return move_next (MOVE_ON_RIGHT, near_border); -} - -/* - * MOVE_ON_LEFT =failed_or_blocked=> - * => MOVE_ON_LEFT + * MOVE_ON_LEFT =failed_or_blocked_or_near_border=> + * => MOVE_ON_RIGHT * The position is fail again, it will try to reach another one. */ fsm_branch_t -move__MOVE_ON_LEFT__failed_or_blocked (void) +move__MOVE_ON_LEFT__failed_or_blocked_or_near_border (void) { - return move_next (MOVE_ON_LEFT, failed_or_blocked); + 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)) + { + // call the correct function to go to the right. + asserv_goto (new_pos.x, new_pos.y); + } + + return move_next (MOVE_ON_LEFT, failed_or_blocked_or_near_border); } /* @@ -96,18 +137,8 @@ move__MOVE_ON_LEFT__failed_or_blocked (void) fsm_branch_t move__MOVE_ON_LEFT__reached (void) { + asserv_goto (move_data.position_x, move_data.position_y); return move_next (MOVE_ON_LEFT, reached); } -/* - * MOVE_ON_LEFT =near_border=> - * => MOVE_ON_RIGHT - * The robot is now too near of the border. - */ -fsm_branch_t -move__MOVE_ON_LEFT__near_border (void) -{ - return move_next (MOVE_ON_LEFT, near_border); -} - diff --git a/digital/io/src/test/move/main.c b/digital/io/src/test/move/main.c index 60f62aa3..ea6eb4e4 100644 --- a/digital/io/src/test/move/main.c +++ b/digital/io/src/test/move/main.c @@ -24,9 +24,32 @@ * }}} */ #include "common.h" #include "../../fsm.h" +#include "../../asserv.h" +#include "../../move.h" #include +/** + * Status structure maintains by the update command. + */ +typedef struct asserv_struct_s +{ + /** Status flags. */ + uint8_t status; + /** Sequence number. */ + uint8_t seq; + /** Bot position. */ + asserv_position_t position; + /** Arm position. */ + uint16_t arm_position; +} asserv_struct_s; + +/** + * Status variable. + */ +asserv_struct_s asserv_status; + + void move_print_test (fsm_t *move) { @@ -55,6 +78,9 @@ move_print_test (fsm_t *move) int main (void) { + move_data.position_x = asserv_status.position.x = 1500; + move_data.position_y = asserv_status.position.y = 1050; + fsm_init (&move_fsm); fsm_handle_event (&move_fsm, MOVE_EVENT_ok); @@ -64,15 +90,9 @@ main (void) fsm_handle_event (&move_fsm, MOVE_EVENT_failed_or_blocked); move_print_test (&move_fsm); - fsm_handle_event (&move_fsm, MOVE_EVENT_failed_or_blocked); + fsm_handle_event (&move_fsm, MOVE_EVENT_failed_or_blocked_or_near_border); move_print_test (&move_fsm); - fsm_handle_event (&move_fsm, MOVE_EVENT_near_border); - move_print_test (&move_fsm); - - fsm_handle_event (&move_fsm, MOVE_EVENT_failed_or_blocked); - move_print_test (&move_fsm); - fsm_handle_event (&move_fsm, MOVE_EVENT_reached); move_print_test (&move_fsm); @@ -88,14 +108,17 @@ main (void) return 0; } -//void -//asserv_set_x_position (uint32_t position) -//{ -// printf ("X position : %d\n", position); -//} -// -//void -//asserv_set_y_position (int32_t y) -//{ -// printf ("Y position : %d\n", y); -//} +void +asserv_goto (uint32_t x, uint32_t y) +{ + printf ("x : %d\n", x); + printf ("y : %d\n", y); +} + +void +asserv_get_position (asserv_position_t *pos) +{ + pos->x = asserv_status.position.x; + pos->y = asserv_status.position.y; +} + -- cgit v1.2.3