summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorJérémy Dufour2008-04-30 01:38:38 +0200
committerJérémy Dufour2008-04-30 01:38:38 +0200
commitdf6bd29c4b37a0d1ffed7c4591484e49d9ca24ba (patch)
treeb3d3e51a0a20918a8e46ccb406a1c7bdcf9c038e
parentec7e026ab256ccd5a8d345a902940b696b96114f (diff)
* digital/io/src
- rewrite the move FSM ; - integrate it into the main loop with the sharp and path module.
-rw-r--r--digital/io/src/main.c62
-rw-r--r--digital/io/src/main.h7
-rw-r--r--digital/io/src/move.c12
-rw-r--r--digital/io/src/move.fsm85
-rw-r--r--digital/io/src/move.h49
-rw-r--r--digital/io/src/move_cb.c328
6 files changed, 352 insertions, 191 deletions
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"
@@ -70,6 +71,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.
*/
uint8_t main_stats_sharps, main_stats_sharps_cpt;
@@ -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 (&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);
}