From cb9ddc2d8162ac2ec0d82579657b984335ec9aef Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 1 Apr 2010 23:57:40 +0200 Subject: digital/io/src: use one FSM with several active states, closes #83 --- digital/io/src/ai_move_cb.c | 396 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 396 insertions(+) create mode 100644 digital/io/src/ai_move_cb.c (limited to 'digital/io/src/ai_move_cb.c') diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c new file mode 100644 index 00000000..f130f139 --- /dev/null +++ b/digital/io/src/ai_move_cb.c @@ -0,0 +1,396 @@ +/* move_cb.c */ +/* {{{ + * + * 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. + * + * }}} + * move: + * move with avoid obstacle. + */ +#include "common.h" +#include "fsm.h" +#include "ai_cb.h" + +#include "asserv.h" +#include "playground.h" +#include "move.h" +#include "sharp.h" +#include "bot.h" +#include "trace_event.h" + +#include "main.h" /* main_post_event_for_top_fsm */ +#include "modules/math/fixed/fixed.h" /* fixed_* */ +#include "modules/path/path.h" +#include "modules/utils/utils.h" +#include "modules/trace/trace.h" + +#include "debug.host.h" + +/** + * The real radius of the obstacle. + */ +#define MOVE_REAL_OBSTACLE_RADIUS 150 + +/** + * The sharp distance between the bot and the obstacle. + */ +#define MOVE_SHARP_DISTANCE 300 + +/** + * The distance between the axis of the bot and the front sharp. + */ +#define MOVE_AXIS_FRONT_SHARP 150 + +/** + * The standard distance of the obstacle. + */ +#define MOVE_OBSTACLE_DISTANCE \ + (MOVE_REAL_OBSTACLE_RADIUS + MOVE_SHARP_DISTANCE + MOVE_AXIS_FRONT_SHARP) + +/** + * The radius of the obstacle for the path module. + * It corresponds to the real radius of the obstacle plus the distance you + * want to add to avoid it. + */ +#define MOVE_OBSTACLE_RADIUS (MOVE_REAL_OBSTACLE_RADIUS + 250) + +/** + * The generic validity time (in term of number of cycles). + */ +#define MOVE_OBSTACLE_VALIDITY (6 * 225) + +/** + * Cycles count to ignore sharp event in the main loop. + */ +#define MOVE_MAIN_IGNORE_SHARP_EVENT (3 * 225) + +/** + * Number of cycles to wait before trying to read the sharps values again when + * we are stopped. + */ +#define MOVE_WAIT_TIME_FOR_POOLING_SHARP (MOVE_MAIN_IGNORE_SHARP_EVENT) + +/** + * A detection offset for the sharps. + */ +#define MOVE_DETECTION_OFFSET 250 + +/** + * Verify after the computation of the obstacle, this shall only be called + * after the function move_compute_obstacle_position. + * \return true if the position computed is in the table, false otherwise. + */ +uint8_t +move_obstacle_in_table (move_position_t pos) +{ + if ((pos.x <= PG_WIDTH - MOVE_DETECTION_OFFSET) + && (pos.x > MOVE_DETECTION_OFFSET) + && (pos.y <= PG_LENGTH - MOVE_DETECTION_OFFSET) + && (pos.y > MOVE_DETECTION_OFFSET)) + return 0x1; + else + return 0x0; +} + +/** + * Easier function to get the next intermediate position from the path module. + * @param dst new destination position computed by the path module + * @return + * - 0 if no path could be found ; + * - 1 if a path has been found. + * - 2 already at final place. + */ +uint8_t +move_get_next_position (move_position_t *dst) +{ + /* Are we at the final position. */ + if (move_data.final_move) + return 2; + /* 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 (); + + /* If the path is found. */ + if (path_get_next (&dst->x, &dst->y) != 0) + { + /* If it is not the last position. */ + if (dst->x != move_data.final.x || dst->y != move_data.final.y) + { + /* Not final position. */ + move_data.final_move = 0; + /* Goto without angle. */ + asserv_goto (dst->x, dst->y, + move_data.backward_movement_allowed); + } + else + { + /* Final position. */ + move_data.final_move = 1; + /* Goto with angle. */ + asserv_goto_xya (dst->x, dst->y, move_data.final.a, + move_data.backward_movement_allowed); + } + TRACE (TRACE_MOVE__GO_TO, (u16) current_pos.x, (u16) current_pos.y, + current_pos.a, dst->x, dst->y, move_data.final.a); + /* Reset try counter. */ + move_data.try_again_counter = 3; + return 1; + } + else + { + /* Error, not final move. */ + move_data.final_move = 0; + return 0; + } +} + +/** + * 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) +{ + int16_t dist; + + /* Convert the angle */ + uint32_t angle = cur.a; + angle = angle << 8; + + /* Dirty fix: distance of the obstacle. */ + dist = BOT_LENGTH / 2 + 350; + /* Invert if last movement was backward. */ + if (asserv_get_last_moving_direction () == 2) + { + dist = -dist; + } + + /* X */ + obstacle->x = cur.x + fixed_mul_f824 (fixed_cos_f824 (angle), + dist); + /* Y */ + obstacle->y = cur.y + fixed_mul_f824 (fixed_sin_f824 (angle), + dist); +} + +/** + * 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 */ + /* only id the obstacle is in the table */ + if (move_obstacle_in_table (move_data.obstacle)) + { + path_obstacle (0, move_data.obstacle.x, move_data.obstacle.y, + MOVE_OBSTACLE_RADIUS, 0, MOVE_OBSTACLE_VALIDITY); + DPRINTF ("Obstacle pos x : %d, pos y : %d\n", move_data.obstacle.x, + move_data.obstacle.y); + TRACE (TRACE_MOVE__OBSTACLE, move_data.obstacle.x, + move_data.obstacle.y); + TRACE (TRACE_MOVE__SHARP, sharp_get_raw (0), sharp_get_raw (1), + sharp_get_raw (2), sharp_get_raw (3), sharp_get_raw (4)); + } + else + { + DPRINTF ("Obstacle Ignored pos x : %d, pos y : %d\n", + move_data.obstacle.x, + move_data.obstacle.y); + } +} + +/* + * MOVE_IDLE =move_start=> + * => MOVE_MOVING + * ask the asserv to go to the computed position. + */ +fsm_branch_t +ai__MOVE_IDLE__move_start (void) +{ + /* ask the asserv to go to the computed position. */ + move_get_next_position (&move_data.intermediate); + return ai_next (MOVE_IDLE, move_start); +} + +/* + * MOVE_MOVING =bot_move_succeed=> + * we_are_at_final_position => MOVE_IDLE + * post an event for the top FSM to tell it we have finished. + * position_intermediary => MOVE_MOVING + * get next position computed by the path module. + * if next position is the final, use a goto_xya. + * otherwise go to the next intermediate position with goto. + * no_intermediate_path_found => MOVE_IDLE + * post an event for the top FSM to generate a failure. + */ +fsm_branch_t +ai__MOVE_MOVING__bot_move_succeed (void) +{ + uint8_t ret = move_get_next_position (&move_data.intermediate); + if (ret == 2) + { + /* Post an event for the top FSM to tell it we have finished. */ + main_post_event_for_top_fsm = AI_EVENT_move_fsm_succeed; + return ai_next_branch (MOVE_MOVING, bot_move_succeed, we_are_at_final_position); + } + else if (ret == 1) + { + /* Nothing to do. */ + return ai_next_branch (MOVE_MOVING, bot_move_succeed, position_intermediary); + } + else + { + /* Post an event for the top FSM to generate a failure. */ + main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed; + return ai_next_branch (MOVE_MOVING, bot_move_succeed, no_intermediate_path_found); + } +} + +/* + * MOVE_MOVING =bot_move_failed=> + * => MOVE_MOVING_BACKWARD_TO_TURN_FREELY + * compute the obstacle position. + * move backward to turn freely. + */ +fsm_branch_t +ai__MOVE_MOVING__bot_move_failed (void) +{ + /* Compute the obstacle position. */ + move_obstacle_here (); + /* Move backward to turn freely. */ + asserv_move_linearly (asserv_get_last_moving_direction () == 1 ? + - 300 : 300); + return ai_next (MOVE_MOVING, bot_move_failed); +} + +/* + * MOVE_MOVING =obstacle_in_front=> + * => MOVE_WAIT_FOR_CLEAR_PATH + * stop the bot. + */ +fsm_branch_t +ai__MOVE_MOVING__obstacle_in_front (void) +{ + /* Stop the bot. */ + asserv_stop_motor (); + return ai_next (MOVE_MOVING, obstacle_in_front); +} + +/* + * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=> + * intermediate_path_found => MOVE_MOVING + * get next intermediate position from path module. + * no_intermediate_path_found => MOVE_IDLE + * post an event for the top FSM to generate a failure. + */ +fsm_branch_t +ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void) +{ + uint8_t ret = move_get_next_position (&move_data.intermediate); + if (ret == 1) + { + /* Nothing to do. */ + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, intermediate_path_found); + } + else + { + /* Post an event for the top FSM to generate a failure. */ + main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed; + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_intermediate_path_found); + } +} + +/* + * MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_failed=> + * intermediate_path_found => MOVE_MOVING + * get next intermediate position from path module + * no_intermediate_path_found => MOVE_WAIT_FOR_CLEAR_PATH + * nothing to do. + */ +fsm_branch_t +ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void) +{ + uint8_t ret = move_get_next_position (&move_data.intermediate); + if (ret == 1) + { + /* Nothing to do. */ + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, intermediate_path_found); + } + else + { + /* Nothing to do. */ + return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_intermediate_path_found); + } +} + +/* + * MOVE_WAIT_FOR_CLEAR_PATH =state_timeout=> + * no_more_obstacle_or_next_position => MOVE_MOVING + * get next position computed by the path module. + * if next position is the final, use a goto_xya. + * otherwise go to the next intermediate position with goto. + * obstacle_and_no_intermediate_path_found_and_try_again => MOVE_WAIT_FOR_CLEAR_PATH + * decrement counter. + * obstacle_and_no_intermediate_path_found_and_no_try_again => MOVE_IDLE + * post an event for the top FSM to generate a failure. + */ +fsm_branch_t +ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void) +{ + if (sharp_path_obstrued (asserv_get_last_moving_direction ())) + move_obstacle_here (); + uint8_t ret = move_get_next_position (&move_data.intermediate); + if (ret == 1) + { + /* Go to position. */ + return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, + no_more_obstacle_or_next_position); + } + else + { + /* Error, no new position, should we try again? */ + if (--move_data.try_again_counter == 0) + { + /* Post an event for the top FSM to generate a failure. */ + main_post_event_for_top_fsm = AI_EVENT_move_fsm_failed; + return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, + obstacle_and_no_intermediate_path_found_and_no_try_again); + } + else + return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, + obstacle_and_no_intermediate_path_found_and_try_again); + } +} + + -- cgit v1.2.3