summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/move_cb.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io/src/move_cb.c')
-rw-r--r--digital/io/src/move_cb.c328
1 files changed, 214 insertions, 114 deletions
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 (&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 ();
- 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 (&current);
+ /* 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 (&current_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 (&current_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);
}