From df6bd29c4b37a0d1ffed7c4591484e49d9ca24ba Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 30 Apr 2008 01:38:38 +0200 Subject: * digital/io/src - rewrite the move FSM ; - integrate it into the main loop with the sharp and path module. --- digital/io/src/main.c | 62 ++++++--- digital/io/src/main.h | 7 + digital/io/src/move.c | 12 +- digital/io/src/move.fsm | 85 +++++++----- digital/io/src/move.h | 49 ++++--- digital/io/src/move_cb.c | 328 +++++++++++++++++++++++++++++++---------------- 6 files changed, 352 insertions(+), 191 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 9c218336..d1be0dfd 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -46,6 +46,7 @@ #include "chrono.h" /* chrono_end_match */ #include "gutter.h" /* gutter_generate_wait_finished_event */ #include "sharp.h" /* sharp module */ +#include "path.h" /* path module */ #include "io.h" @@ -69,6 +70,11 @@ enum team_color_e bot_color; */ uint8_t main_post_event_for_top_fsm = 0xFF; +/** + * Do not generate sharps event for FSM during a certain count of cycles. + */ +uint16_t main_sharp_ignore_event; + /** * Sharps stats counters. */ @@ -107,6 +113,8 @@ main_init (void) trap_init (); /* Switch module */ switch_init (); + /* Path module */ + path_init (); /* Start the top FSM */ top_start (); /* Sharp module */ @@ -160,11 +168,18 @@ main_loop (void) } else { - /* First update modules */ + /* First, update modules */ /* Update switch module */ switch_update (); + /* Update path module */ + path_decay (); /* Sharps module */ - if (++main_sharp_freq_counter_ == MAIN_SHARP_UPDATE_FREQ) + /* Update the ignore sharp event flag */ + if (main_sharp_ignore_event) + main_sharp_ignore_event--; + /* Update sharp module if required and only every + * MAIN_SHARP_UPDATE_FREQ cycles */ + if ((main_sharp_ignore_event == 0) && (++main_sharp_freq_counter_ == MAIN_SHARP_UPDATE_FREQ)) { /* Update sharps */ sharp_update (0xff); @@ -246,25 +261,34 @@ main_loop (void) FSM_HANDLE_EVENT (&top_fsm, save_event); } /* Sharps event for move FSM */ - uint8_t moving_direction = asserv_get_moving_direction (); - if (moving_direction) + /* If we do not need to ignore sharp event */ + if (!main_sharp_ignore_event) { - if (moving_direction == 1) - { - /* Front only */ - if (sharp_get_interpreted (SHARP_FRONT_LEFT) || - sharp_get_interpreted (SHARP_FRONT_RIGHT)) - /* Generate an event for move FSM */ - FSM_HANDLE_EVENT (&move_fsm, - MOVE_EVENT_bot_move_obstacle); - } - else + /* Get the current direction of the bot */ + uint8_t moving_direction = asserv_get_moving_direction (); + /* If we are moving */ + if (moving_direction) { - /* Back only */ - if (sharp_get_interpreted (SHARP_BACK_LEFT) || - sharp_get_interpreted (SHARP_BACK_RIGHT)) - /* Generate an event for move FSM */ - FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_bot_move_obstacle); + /* If we are moving forward */ + if (moving_direction == 1) + { + /* Use only front sharps */ + if (sharp_get_interpreted (SHARP_FRONT_LEFT) || + sharp_get_interpreted (SHARP_FRONT_MIDDLE) || + sharp_get_interpreted (SHARP_FRONT_RIGHT)) + /* Generate an event for move FSM */ + FSM_HANDLE_EVENT (&move_fsm, + MOVE_EVENT_bot_move_obstacle); + } + /* If we are moving backward */ + else if (moving_direction == 2) + { + /* Use only back sharps */ + if (sharp_get_interpreted (SHARP_BACK_LEFT) || + sharp_get_interpreted (SHARP_BACK_RIGHT)) + /* Generate an event for move FSM */ + FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_bot_move_obstacle); + } } } /* TODO: Check other sensors */ diff --git a/digital/io/src/main.h b/digital/io/src/main.h index 40bf1f9d..24359c6d 100644 --- a/digital/io/src/main.h +++ b/digital/io/src/main.h @@ -33,4 +33,11 @@ */ extern uint8_t main_post_event_for_top_fsm; +/** + * Do not generate sharps event for FSM during a certain count of cycles. + * This flag is used by the move FSM to prevent it from being alerted by the + * main loop that there is an object in from of the bot. + */ +extern uint16_t main_sharp_ignore_event; + #endif /* main_h */ diff --git a/digital/io/src/move.c b/digital/io/src/move.c index 53709d07..2c0a0c5e 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -27,16 +27,18 @@ #include "move.h" #include "fsm.h" +/** + * Internal data used by the move FSM. + */ struct move_data_t move_data; -/** Start a move FSM. */ +/* Go to a position with the start FSM. */ void -move_start (uint32_t position_x, uint32_t position_y) +move_start (uint16_t position_x, uint16_t position_y) { /* Set parameters. */ - move_data.position_x = position_x; - move_data.position_y = position_y; - move_data.nb_obstacle = 0; + move_data.final.x = position_x; + move_data.final.y = position_y; /* Start the FSM. */ fsm_init (&move_fsm); fsm_handle_event (&move_fsm, MOVE_EVENT_start); diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index 9eaaf02b..ab55d1d4 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -1,46 +1,67 @@ -#Move FSM. +# Move FSM +# This FSM is responsible to move the bot from the current position to the +# desired one. It will try to avoid obstacle and manage the case when the +# asserv is blocked. move States: IDLE - DESIRED_POSITION - BACKWARD - GO_AWAY - STOP_WAIT + waiting for the beginning of the move FSM + MOVING_TO_FINAL_POSITION + moving to the final position + MOVING_BACKWARD + moving backward to go away from what is blocking the bot + MOVING_TO_INTERMEDIATE_POSITION + moving to an intermediate position to try avoiding the obstacle Events: start - bot_move_succeed + initialize the FSM bot_move_failed + the bot movement failed (blocked by something for example) + bot_move_succeed + the bot has finished to moved and arrived at the desired position bot_move_obstacle - wait_finished + the bot has seen something (with the sharps) IDLE: - start -> DESIRED_POSITION - Save the destination position and put obstacles number to 0. - -DESIRED_POSITION: - 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. + start -> MOVING_TO_FINAL_POSITION + ask the asserv to go to the final position -BACKWARD: - bot_move_succeed-> GO_AWAY - Launch the pathfinder and go to the next position. +MOVING_TO_FINAL_POSITION: + bot_move_succeed -> IDLE + post an event for the top FSM to tell we have finished + bot_move_failed -> MOVING_BACKWARD + store the current position of the obstacle + move backward to turn freely + bot_move_obstacle -> MOVING_TO_INTERMEDIATE_POSITION + compute the obstacle position + give the obstacle position to the path module + give the current position of the bot to the path module + get the intermediate position from path module -STOP_WAIT: - wait_finished-> GO_AWAY - Launch the pathfinder and go to the next position. +MOVING_BACKWARD: +# TODO +# We ignore obstacle when moving backward + bot_move_succeed -> MOVING_TO_INTERMEDIATE_POSITION + give the obstacle position to the path module + give the current position of the bot to the path module + get the intermediate position from path module + bot_move_failed -> MOVING_TO_INTERMEDIATE_POSITION + do the same as when we succeed + give the obstacle position to the path module + give the current position of the bot to the path module + get the intermediate position from path module -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. +MOVING_TO_INTERMEDIATE_POSITION: + bot_move_obstacle -> . + compute the obstacle position + give the obstacle position to the path module + go to the next intermediate position computed by the path module + bot_move_succeed: final_position -> IDLE + post an event for the top FSM to tell we have finished + bot_move_succeed: position_intermediary -> . + go to the next intermediate position computed by the path module + bot_move_failed -> MOVING_BACKWARD + store the current position of the obstacle + move backward to turn freely diff --git a/digital/io/src/move.h b/digital/io/src/move.h index b5d2a483..dc2c2603 100644 --- a/digital/io/src/move.h +++ b/digital/io/src/move.h @@ -25,34 +25,41 @@ * * }}} */ -#include "asserv.h" - -#define MOVE_OBSTACLE_RADIUS 100 -#define MOVE_OBSTACLE_VALIDITY 650 +/** + * A position. + */ +typedef struct move_position_t +{ + /** X position. */ + uint16_t x; + /** Y position. */ + uint16_t y; +} move_position_t; -/** move FSM associated data. */ +/** + * Move FSM associated data. + */ struct move_data_t { - /* Desired x position to reached. */ - uint16_t position_x; - /* Desired y position to reached. */ - 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; + /** Final position. */ + move_position_t final; + /** Intermediate position. */ + move_position_t intermediate; + /** Obstacle position. */ + move_position_t obstacle; }; - -/** move global. */ +/** + * Move global data. + */ extern struct move_data_t move_data; -/** Start a move FSM. */ +/** + * Go to a position with the start FSM. + * @param position_x the X position. + * @param position_y the Y position. + */ void -move_start (uint32_t position_x, uint32_t position_y); +move_start (uint16_t position_x, uint16_t position_y); #endif /* move_h */ diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index dbbc637e..fbe06d63 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -1,183 +1,283 @@ -/* - * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! +/* move_cb.c */ +/* {{{ * - * Skeleton for move callbacks implementation. + * Copyright (C) 2008 Nélio Laranjeiro * - * - */ + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ #include "common.h" #include "fsm.h" #include "move_cb.h" + #include "path.h" #include "asserv.h" #include "playground.h" #include "move.h" #include "main.h" /* main_post_event_for_top_fsm */ +#include "modules/math/fixed/fixed.h" /* fixed_* */ -/* - * 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. +/** + * The standard distance of the obstacle. */ -fsm_branch_t -move__GO_AWAY__bot_move_succeed (void) +#define MOVE_OBSTACLE_DISTANCE 300 + +/** + * The generic radius of the obstacle. + */ +#define MOVE_OBSTACLE_RADIUS 150 +/** + * The generic validity time (in term of number of cyles). + */ +#define MOVE_OBSTACLE_VALIDITY 2000 + +/** + * Cycles count to ignore sharp event in the main loop. + */ +#define MOVE_MAIN_IGNORE_SHARP_EVENT 1250 + +/** + * Easier function to get the next intermediate positon from the path module. + * @param dst new destination position computed by the path module + */ +void +move_get_next_position (move_position_t *dst) { - 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 (); + /* Get the current position */ + asserv_position_t current_pos; + asserv_get_position (¤t_pos); + /* Give the current position of the bot to the path module */ + path_endpoints (current_pos.x, current_pos.y, + move_data.final.x, move_data.final.y); + /* Update the path module */ + 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); + /* Retrieve next path coordinate */ + if (!path_get_next (&dst->x, &dst->y)) + { + /* If it failed, try original destination */ + dst->x = move_data.final.x; + dst->y = move_data.final.y; } + main_sharp_ignore_event = MOVE_MAIN_IGNORE_SHARP_EVENT; +} + +/** + * Compute the obstacle position assuming it is right in front of us. + * @param cur current position + * @param obstacle the obstacle position computed + */ +void +move_compute_obstacle_position (asserv_position_t cur, + move_position_t *obstacle) +{ + /* Convert the angle */ + uint32_t angle = cur.a; + angle = angle << 2; + /* X */ + obstacle->x = cur.x + fixed_mul_f824 (fixed_cos_f824 (angle), + MOVE_OBSTACLE_DISTANCE); + /* Y */ + obstacle->y = cur.y + fixed_mul_f824 (fixed_sin_f824 (angle), + MOVE_OBSTACLE_DISTANCE); +} +/** + * Unique function to compute the obstacle position from here. + */ +void +move_obstacle_here (void) +{ + /* Get the current position of the bot */ + asserv_position_t current; + asserv_get_position (¤t); + /* Compute the obstacle position */ + move_compute_obstacle_position (current, &move_data.obstacle); + /* Give it to the path module */ + path_obstacle (0, move_data.obstacle.x, move_data.obstacle.y, + MOVE_OBSTACLE_RADIUS, MOVE_OBSTACLE_VALIDITY); +} + +/** + * Unique function after moving backward to have unique code. + */ +void +move_after_moving_backward (void) +{ + /* Give the current position of the bot to the path module */ + move_get_next_position (&move_data.intermediate); + /* Go to the next position */ + asserv_goto (move_data.intermediate.x, move_data.intermediate.y); } /* - * GO_AWAY =bot_move_failed=> - * => BACKWARD - * Store the current position, and go backward. + * IDLE =start=> + * => MOVING_TO_FINAL_POSITION + * ask the asserv to go to the final position */ fsm_branch_t -move__GO_AWAY__bot_move_failed (void) +move__IDLE__start (void) { - asserv_move_linearly (-PG_MOVE_DISTANCE); - return move_next (GO_AWAY, bot_move_failed); + /* Go to the destination position */ + asserv_goto (move_data.final.x, move_data.final.y); + return move_next (IDLE, start); } /* - * GO_AWAY =bot_move_obstacle=> - * => STOP_WAIT - * Go to the stop wait event. + * MOVING_TO_INTERMEDIATE_POSITION =bot_move_succeed=> + * final_position => IDLE + * post an event for the top FSM to tell we have finished + * position_intermediary => MOVING_TO_INTERMEDIATE_POSITION + * go to the next intermediate position computed by the path module */ fsm_branch_t -move__GO_AWAY__bot_move_obstacle (void) +move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_succeed (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); + if ((move_data.final.x == move_data.intermediate.x) && + (move_data.final.y == move_data.intermediate.y)) + { + /* Post an event for the top FSM to tell we have finished */ + main_post_event_for_top_fsm = TOP_EVENT_move_fsm_finished; + return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, final_position); + } + else + { + /* Get next position */ + move_get_next_position (&move_data.intermediate); + /* Go to the next intermediate position */ + asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, position_intermediary); + } } /* - * STOP_WAIT =wait_finished=> - * => GO_AWAY - * Launch the pathfinder and go to the next position. + * MOVING_TO_INTERMEDIATE_POSITION =bot_move_obstacle=> + * => MOVING_TO_INTERMEDIATE_POSITION + * compute the obstacle position + * give the obstacle position to the path module + * go to the next intermediate position computed by the path module */ fsm_branch_t -move__STOP_WAIT__wait_finished (void) +move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_obstacle (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); + /* Compute obstacle position */ + move_obstacle_here (); + /* Get next position */ + move_get_next_position (&move_data.intermediate); + /* Go to the next intermediate position */ + asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + return move_next (MOVING_TO_INTERMEDIATE_POSITION, bot_move_obstacle); } /* - * IDLE =start=> - * => DESIRED_POSITION - * Save the destination position and put obstacles number to 0. + * MOVING_TO_INTERMEDIATE_POSITION =bot_move_failed=> + * => MOVING_BACKWARD + * store the current position of the obstacle + * move backward to turn freely */ fsm_branch_t -move__IDLE__start (void) +move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_failed (void) { - // Try to go the desired position. - asserv_goto (move_data.position_x, move_data.position_y); - return move_next (IDLE, start); + /* Compute obstacle position */ + move_obstacle_here (); + /* Go backward */ + asserv_move_linearly (-PG_MOVE_DISTANCE); + return move_next (MOVING_TO_INTERMEDIATE_POSITION, bot_move_failed); } /* - * BACKWARD =bot_move_succeed=> - * => GO_AWAY - * Launch the pathfinder and go to the next position. + * MOVING_BACKWARD =bot_move_failed=> + * => MOVING_TO_INTERMEDIATE_POSITION + * do the same as when we succeed + * give the obstacle position to the path module + * give the current position of the bot to the path module + * get the intermediate position from path module */ fsm_branch_t -move__BACKWARD__bot_move_succeed (void) +move__MOVING_BACKWARD__bot_move_failed (void) { - asserv_position_t current_pos; - - // 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 (); - - // 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); + /* Call generic function */ + move_after_moving_backward (); + return move_next (MOVING_BACKWARD, bot_move_failed); } /* - * 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. + * MOVING_BACKWARD =bot_move_succeed=> + * => MOVING_TO_INTERMEDIATE_POSITION + * give the obstacle position to the path module + * give the current position of the bot to the path module + * get the intermediate position from path module */ fsm_branch_t -move__DESIRED_POSITION__bot_move_failed (void) +move__MOVING_BACKWARD__bot_move_succeed (void) { - // Go backward linearly. - asserv_move_linearly (-PG_MOVE_DISTANCE); + /* Call generic function */ + move_after_moving_backward (); + return move_next (MOVING_BACKWARD, bot_move_succeed); +} - return move_next (DESIRED_POSITION, bot_move_failed); +/* + * MOVING_TO_FINAL_POSITION =bot_move_failed=> + * => MOVING_BACKWARD + * store the current position of the obstacle + * move backward to turn freely + */ +fsm_branch_t +move__MOVING_TO_FINAL_POSITION__bot_move_failed (void) +{ + /* Compute obstacle position */ + move_obstacle_here (); + /* Move backward to turn freely */ + asserv_move_linearly (-PG_MOVE_DISTANCE); + return move_next (MOVING_TO_FINAL_POSITION, bot_move_failed); } /* - * DESIRED_POSITION =bot_move_succeed=> + * MOVING_TO_FINAL_POSITION =bot_move_succeed=> * => IDLE - * Destination position reached by the bot. Set the flag of the top event to inform it. + * post an event for the top FSM to tell we have finished */ fsm_branch_t -move__DESIRED_POSITION__bot_move_succeed (void) +move__MOVING_TO_FINAL_POSITION__bot_move_succeed (void) { + /* Post an event for the top FSM to tell we have finished */ main_post_event_for_top_fsm = TOP_EVENT_move_fsm_finished; - return move_next (DESIRED_POSITION, bot_move_succeed); + return move_next (MOVING_TO_FINAL_POSITION, bot_move_succeed); } /* - * DESIRED_POSITION =bot_move_obstacle=> - * => STOP_WAIT - * Stop the bot. + * MOVING_TO_FINAL_POSITION =bot_move_obstacle=> + * => MOVING_TO_INTERMEDIATE_POSITION + * compute the obstacle position + * give the obstacle position to the path module + * give the current position of the bot to the path module + * get the intermediate position from path module */ fsm_branch_t -move__DESIRED_POSITION__bot_move_obstacle (void) +move__MOVING_TO_FINAL_POSITION__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 ++; - - return move_next (DESIRED_POSITION, bot_move_obstacle); + /* Compute obstacle position */ + move_obstacle_here (); + /* Get next position */ + move_get_next_position (&move_data.intermediate); + /* Go to the next intermediate position */ + asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + return move_next (MOVING_TO_FINAL_POSITION, bot_move_obstacle); } -- cgit v1.2.3