summaryrefslogtreecommitdiff
path: root/digital/io/src/move_cb.c
diff options
context:
space:
mode:
authorNicolas Schodet2010-04-01 23:57:40 +0200
committerNicolas Schodet2010-04-01 23:57:40 +0200
commitcb9ddc2d8162ac2ec0d82579657b984335ec9aef (patch)
tree952c6193ffb4b880f71990628a830825cd9d4f6e /digital/io/src/move_cb.c
parent80b3da91dac28a5895a8cb9f0477a7008365dd56 (diff)
digital/io/src: use one FSM with several active states, closes #83
Diffstat (limited to 'digital/io/src/move_cb.c')
-rw-r--r--digital/io/src/move_cb.c393
1 files changed, 0 insertions, 393 deletions
diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c
deleted file mode 100644
index 900842b9..00000000
--- a/digital/io/src/move_cb.c
+++ /dev/null
@@ -1,393 +0,0 @@
-/* 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.
- *
- * }}} */
-#include "common.h"
-#include "fsm.h"
-#include "move_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 (&current_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 (&current);
- /* 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);
- }
-}
-
-/*
- * IDLE =start=>
- * => MOVING
- * ask the asserv to go to the computed position.
- */
-fsm_branch_t
-move__IDLE__start (void)
-{
- /* ask the asserv to go to the computed position. */
- move_get_next_position (&move_data.intermediate);
- return move_next (IDLE, start);
-}
-
-/*
- * MOVING =bot_move_succeed=>
- * we_are_at_final_position => IDLE
- * post an event for the top FSM to tell it we have finished.
- * position_intermediary => 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 => IDLE
- * post an event for the top FSM to generate a failure.
- */
-fsm_branch_t
-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 = TOP_EVENT_move_fsm_succeed;
- return move_next_branch (MOVING, bot_move_succeed, we_are_at_final_position);
- }
- else if (ret == 1)
- {
- /* Nothing to do. */
- return move_next_branch (MOVING, bot_move_succeed, position_intermediary);
- }
- else
- {
- /* Post an event for the top FSM to generate a failure. */
- main_post_event_for_top_fsm = TOP_EVENT_move_fsm_failed;
- return move_next_branch (MOVING, bot_move_succeed, no_intermediate_path_found);
- }
-}
-
-/*
- * MOVING =bot_move_failed=>
- * => MOVING_BACKWARD_TO_TURN_FREELY
- * compute the obstacle position.
- * move backward to turn freely.
- */
-fsm_branch_t
-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 move_next (MOVING, bot_move_failed);
-}
-
-/*
- * MOVING =obstacle_in_front=>
- * => WAIT_FOR_CLEAR_PATH
- * stop the bot.
- */
-fsm_branch_t
-move__MOVING__obstacle_in_front (void)
-{
- /* Stop the bot. */
- asserv_stop_motor ();
- return move_next (MOVING, obstacle_in_front);
-}
-
-/*
- * MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=>
- * intermediate_path_found => MOVING
- * get next intermediate position from path module.
- * no_intermediate_path_found => IDLE
- * post an event for the top FSM to generate a failure.
- */
-fsm_branch_t
-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 move_next_branch (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 = TOP_EVENT_move_fsm_failed;
- return move_next_branch (MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, no_intermediate_path_found);
- }
-}
-
-/*
- * MOVING_BACKWARD_TO_TURN_FREELY =bot_move_failed=>
- * intermediate_path_found => MOVING
- * get next intermediate position from path module
- * no_intermediate_path_found => WAIT_FOR_CLEAR_PATH
- * nothing to do.
- */
-fsm_branch_t
-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 move_next_branch (MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, intermediate_path_found);
- }
- else
- {
- /* Nothing to do. */
- return move_next_branch (MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_intermediate_path_found);
- }
-}
-
-/*
- * WAIT_FOR_CLEAR_PATH =state_timeout=>
- * no_more_obstacle_or_next_position => 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 => WAIT_FOR_CLEAR_PATH
- * decrement counter.
- * obstacle_and_no_intermediate_path_found_and_no_try_again => IDLE
- * post an event for the top FSM to generate a failure.
- */
-fsm_branch_t
-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 move_next_branch (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 = TOP_EVENT_move_fsm_failed;
- return move_next_branch (WAIT_FOR_CLEAR_PATH, state_timeout,
- obstacle_and_no_intermediate_path_found_and_no_try_again);
- }
- else
- return move_next_branch (WAIT_FOR_CLEAR_PATH, state_timeout,
- obstacle_and_no_intermediate_path_found_and_try_again);
- }
-}
-
-