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/move_cb.c | 328 +++++++++++++++++++++++++++++++---------------- 1 file changed, 214 insertions(+), 114 deletions(-) (limited to 'digital/io/src/move_cb.c') 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