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/Makefile | 3 +- digital/io/src/main.c | 4 +- digital/io/src/move.c | 76 +----------------- digital/io/src/move.fsm | 51 ++++++++----- digital/io/src/move.h | 37 +++------ digital/io/src/move_cb.c | 182 +++++++++++++++++++++++++++++--------------- digital/io/src/playground.h | 5 ++ 7 files changed, 174 insertions(+), 184 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index 480d49f2..e230bc89 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -8,7 +8,8 @@ io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c trap.c sharp.c \ getsamples.c getsamples_fsm.c getsamples_cb.c \ gutter_fsm.c gutter_cb.c gutter.c \ move.c move_fsm.c move_cb.c \ - top.c top_fsm.c top_cb.c + top.c top_fsm.c top_cb.c \ + path.c test_path_SOURCES = test_path.c path.c MODULES = proto uart twi utils adc math/fixed test_path_MODULES = math/fixed diff --git a/digital/io/src/main.c b/digital/io/src/main.c index d768836e..ed9d797e 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -158,7 +158,7 @@ main_loop (void) FSM_HANDLE_EVENT (&gutter_fsm, GUTTER_EVENT_bot_move_succeed); FSM_HANDLE_EVENT (&move_fsm, - MOVE_EVENT_reached); + MOVE_EVENT_bot_move_succeed); } else if (move_status == failure) { @@ -168,7 +168,7 @@ main_loop (void) FSM_HANDLE_EVENT (&gutter_fsm, GUTTER_EVENT_bot_move_failed); FSM_HANDLE_EVENT (&move_fsm, - MOVE_EVENT_blocked); + MOVE_EVENT_bot_move_failed); } asserv_status_e arm_status = asserv_last_cmd_ack () ? asserv_arm_cmd_status () : none; 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); - } -} - - diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index 5e2f877b..9eaaf02b 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -4,34 +4,43 @@ move States: IDLE DESIRED_POSITION - MOVE_ON_LEFT - MOVE_ON_RIGHT + BACKWARD + GO_AWAY + STOP_WAIT Events: start - reached - blocked + bot_move_succeed + bot_move_failed + bot_move_obstacle + wait_finished IDLE: start -> DESIRED_POSITION - Tries to reach a position provided by the user. If the position desired can not be reached, it all try to move on the right or the left. + Save the destination position and put obstacles number to 0. DESIRED_POSITION: - reached -> IDLE - The position provided by the user has been reached, the FSM can stop. - blocked:near_left_border -> MOVE_ON_RIGHT - The robot will compute the position from the border and will take the decision to go on the right because the left border is too near. - blocked:near_right_border -> MOVE_ON_LEFT - The robot will compute the position from the border and will take the decision to go on the left because the right border is too near. + bot_move_succeed-> IDLE + Destination position reached by the bot. Set the flag of the top event to inform it. + bot_move_failed-> BACKWARD + Store the current position, initialise the path system i.e. provide the position of the obstacle and request the path system the next position. + bot_move_obstacle-> STOP_WAIT + Stop the bot. -MOVE_ON_LEFT: - reached -> DESIRED_POSITION - The position has been reached. It will now try to reach the position provided by the user. - blocked -> MOVE_ON_RIGHT - The robot will try to go on the left because the previous movement block on the right. +BACKWARD: + bot_move_succeed-> GO_AWAY + Launch the pathfinder and go to the next position. -MOVE_ON_RIGHT: - reached -> DESIRED_POSITION - The position has been reached. It will now try to reach the position provided by the user. - blocked -> MOVE_ON_LEFT - The robot will try to go on the left because the previous movement block on the left. +STOP_WAIT: + wait_finished-> GO_AWAY + Launch the pathfinder and go to the next position. + +GO_AWAY: + bot_move_obstacle -> STOP_WAIT + Go to the stop wait event. + bot_move_succeed: position_desired -> IDLE + The position desired provided on the FSM call had been reached. + bot_move_succeed: position_intermediary -> GO_AWAY + Request the next step to reached to the path finder. + bot_move_failed -> BACKWARD + Store the current position, and go backward. diff --git a/digital/io/src/move.h b/digital/io/src/move.h index 69a2975e..b5d2a483 100644 --- a/digital/io/src/move.h +++ b/digital/io/src/move.h @@ -27,17 +27,24 @@ #include "asserv.h" -/* if the left border is under 500 mm of the actual position do not go there. */ -#define MOVE_BORDER_LEVEL 200 - +#define MOVE_OBSTACLE_RADIUS 100 +#define MOVE_OBSTACLE_VALIDITY 650 /** move FSM associated data. */ struct move_data_t { /* Desired x position to reached. */ - uint32_t position_x; + uint16_t position_x; /* Desired y position to reached. */ - uint32_t position_y; + uint16_t position_y; + /* number of obstacles seen. */ + uint8_t nb_obstacle; + /* Path finder position x. */ + uint16_t path_pos_x; + /* Path finder position y. */ + uint16_t path_pos_y; + /* Position blocked. */ + asserv_position_t pos_blocked_or_failed; }; @@ -48,24 +55,4 @@ 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); - -/** Go to the right. - */ -void -move_go_to_right (void); - -/** Go to the left. - */ -void -move_go_to_left (void); - #endif /* move_h */ diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index a54f9420..2c17927f 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -8,111 +8,173 @@ #include "common.h" #include "fsm.h" #include "move_cb.h" -#include "move.h" +#include "path.h" #include "asserv.h" -#include "main.h" /* main_post_event_for_top_fsm */ +#include "playground.h" +#include "move.h" /* - * IDLE =start=> - * => DESIRED_POSITION - * Tries to reach a position provided by the user. If the position desired can not be reached, it all try to move on the right or the left. + * GO_AWAY =bot_move_succeed=> + * position_intermediary => GO_AWAY + * Request the next step to reached to the path finder. + * position_desired => IDLE + * The position desired provided on the FSM call had been reached. */ fsm_branch_t -move__IDLE__start (void) +move__GO_AWAY__bot_move_succeed (void) { - asserv_goto (move_data.position_x, move_data.position_y); - return move_next (IDLE, start); + if (move_data.position_x == move_data.path_pos_x + && move_data.position_y == move_data.path_pos_y) + return move_next_branch (GO_AWAY, bot_move_succeed, position_desired); + else + { + path_endpoints (move_data.path_pos_x, move_data.path_pos_y, + move_data.position_x, move_data.position_y); + path_update (); + + path_get_next (&move_data.path_pos_x, &move_data.path_pos_y); + asserv_goto (move_data.path_pos_x, move_data.path_pos_y); + return move_next_branch (GO_AWAY, bot_move_succeed, position_intermediary); + } } /* - * MOVE_ON_RIGHT =blocked=> - * => MOVE_ON_LEFT - * The robot will try to go on the left because the previous movement block on the left. + * GO_AWAY =bot_move_failed=> + * => BACKWARD + * Store the current position, and go backward. */ fsm_branch_t -move__MOVE_ON_RIGHT__blocked (void) +move__GO_AWAY__bot_move_failed (void) { - move_go_to_left (); - return move_next (MOVE_ON_RIGHT, blocked); + asserv_move_linearly (-PG_MOVE_DISTANCE); + return move_next (GO_AWAY, bot_move_failed); } /* - * MOVE_ON_RIGHT =reached=> + * GO_AWAY =bot_move_obstacle=> + * => STOP_WAIT + * Go to the stop wait event. + */ +fsm_branch_t +move__GO_AWAY__bot_move_obstacle (void) +{ + asserv_position_t obstacle_pos; + + asserv_stop_motor (); + + // Store the obstacle in the path finder. + asserv_get_position (&obstacle_pos); + path_obstacle (move_data.nb_obstacle, obstacle_pos.x, + obstacle_pos.y, MOVE_OBSTACLE_RADIUS, + MOVE_OBSTACLE_VALIDITY); + move_data.nb_obstacle ++; + + // TODO: Set flag on main to wait. + return move_next (GO_AWAY, bot_move_obstacle); +} + +/* + * STOP_WAIT =wait_finished=> + * => GO_AWAY + * Launch the pathfinder and go to the next position. + */ +fsm_branch_t +move__STOP_WAIT__wait_finished (void) +{ + asserv_position_t current_pos; + + // get the current position. + asserv_get_position (¤t_pos); + + // configure the path finder. + path_endpoints (current_pos.x, current_pos.y, + move_data.position_x, move_data.position_y); + path_update(); + + path_get_next (&move_data.path_pos_x, &move_data.path_pos_y); + asserv_goto (move_data.path_pos_x, move_data.path_pos_y); + return move_next (STOP_WAIT, wait_finished); +} + +/* + * IDLE =start=> * => DESIRED_POSITION - * The position has been reached. It will now try to reach the position provided by the user. + * Save the destination position and put obstacles number to 0. */ fsm_branch_t -move__MOVE_ON_RIGHT__reached (void) +move__IDLE__start (void) { + // Try to go the desired position. asserv_goto (move_data.position_x, move_data.position_y); - return move_next (MOVE_ON_RIGHT, reached); + return move_next (IDLE, start); } /* - * DESIRED_POSITION =blocked=> - * near_right_border => MOVE_ON_LEFT - * The robot will compute the position from the border and will take the decision to go on the left because the right border is too near. - * near_left_border => MOVE_ON_RIGHT - * The robot will compute the position from the border and will take the decision to go on the right because the left border is too near. + * BACKWARD =bot_move_succeed=> + * => GO_AWAY + * Launch the pathfinder and go to the next position. */ fsm_branch_t -move__DESIRED_POSITION__blocked (void) +move__BACKWARD__bot_move_succeed (void) { - asserv_position_t pos; - asserv_position_t new_pos; + asserv_position_t current_pos; - asserv_get_position (&pos); - new_pos = pos; - new_pos.x += MOVE_BORDER_LEVEL ; + // Configure the path finder. + asserv_get_position (¤t_pos); + path_endpoints (current_pos.x, current_pos.y, + move_data.position_x, move_data.position_y); + path_update (); - 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, 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, blocked, near_right_border); - } + // Get the next point to reach. + path_get_next (&move_data.path_pos_x, &move_data.path_pos_y); + asserv_goto (move_data.path_pos_x, move_data.path_pos_y); + return move_next (BACKWARD, bot_move_succeed); } /* - * DESIRED_POSITION =reached=> - * => IDLE - * The position provided by the user has been reached, the FSM can stop. + * DESIRED_POSITION =bot_move_failed=> + * => BACKWARD + * Store the current position, initialise the path system i.e. provide the position of the obstacle and request the path system the next position. */ fsm_branch_t -move__DESIRED_POSITION__reached (void) +move__DESIRED_POSITION__bot_move_failed (void) { - main_post_event_for_top_fsm = TOP_EVENT_move_fsm_finished; - return move_next (DESIRED_POSITION, reached); + // Go backward linearly. + asserv_move_linearly (-PG_MOVE_DISTANCE); + + return move_next (DESIRED_POSITION, bot_move_failed); } /* - * MOVE_ON_LEFT =blocked=> - * => MOVE_ON_RIGHT - * The robot will try to go on the left because the previous movement block on the right. + * DESIRED_POSITION =bot_move_succeed=> + * => IDLE + * Destination position reached by the bot. Set the flag of the top event to inform it. */ fsm_branch_t -move__MOVE_ON_LEFT__blocked (void) +move__DESIRED_POSITION__bot_move_succeed (void) { - move_go_to_right(); - return move_next (MOVE_ON_LEFT, blocked); + return move_next (DESIRED_POSITION, bot_move_succeed); } /* - * MOVE_ON_LEFT =reached=> - * => DESIRED_POSITION - * The position has been reached. It will now try to reach the position provided by the user. + * DESIRED_POSITION =bot_move_obstacle=> + * => STOP_WAIT + * Stop the bot. */ fsm_branch_t -move__MOVE_ON_LEFT__reached (void) +move__DESIRED_POSITION__bot_move_obstacle (void) { - asserv_goto (move_data.position_x, move_data.position_y); - return move_next (MOVE_ON_LEFT, reached); -} + asserv_position_t obstacle_pos; + + asserv_stop_motor (); + // Store the obstacle in the path finder. + asserv_get_position (&obstacle_pos); + path_obstacle (move_data.nb_obstacle, obstacle_pos.x, + obstacle_pos.y, MOVE_OBSTACLE_RADIUS, + MOVE_OBSTACLE_VALIDITY); + move_data.nb_obstacle ++; + + return move_next (DESIRED_POSITION, bot_move_obstacle); +} diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h index 4a3be101..ae97784e 100644 --- a/digital/io/src/playground.h +++ b/digital/io/src/playground.h @@ -114,4 +114,9 @@ #define PG_GUTTER_Y (300) #define PG_GUTTER_A (90 * BOT_ANGLE_DEGREE) +/** + * The backward move for the MOVE_FSM. + */ +#define PG_MOVE_DISTANCE 300 + #endif // playground_h -- cgit v1.2.3