summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/Makefile3
-rw-r--r--digital/io/src/main.c4
-rw-r--r--digital/io/src/move.c76
-rw-r--r--digital/io/src/move.fsm51
-rw-r--r--digital/io/src/move.h37
-rw-r--r--digital/io/src/move_cb.c182
-rw-r--r--digital/io/src/playground.h5
7 files changed, 174 insertions, 184 deletions
diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile
index 480d49f2..e230bc89 100644
--- a/digital/io/src/Makefile
+++ b/digital/io/src/Makefile
@@ -8,7 +8,8 @@ io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c trap.c sharp.c \
getsamples.c getsamples_fsm.c getsamples_cb.c \
gutter_fsm.c gutter_cb.c gutter.c \
move.c move_fsm.c move_cb.c \
- top.c top_fsm.c top_cb.c
+ top.c top_fsm.c top_cb.c \
+ path.c
test_path_SOURCES = test_path.c path.c
MODULES = proto uart twi utils adc math/fixed
test_path_MODULES = math/fixed
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index d768836e..ed9d797e 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -158,7 +158,7 @@ main_loop (void)
FSM_HANDLE_EVENT (&gutter_fsm,
GUTTER_EVENT_bot_move_succeed);
FSM_HANDLE_EVENT (&move_fsm,
- MOVE_EVENT_reached);
+ MOVE_EVENT_bot_move_succeed);
}
else if (move_status == failure)
{
@@ -168,7 +168,7 @@ main_loop (void)
FSM_HANDLE_EVENT (&gutter_fsm,
GUTTER_EVENT_bot_move_failed);
FSM_HANDLE_EVENT (&move_fsm,
- MOVE_EVENT_blocked);
+ MOVE_EVENT_bot_move_failed);
}
asserv_status_e arm_status = asserv_last_cmd_ack ()
? asserv_arm_cmd_status () : none;
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index e360dfa1..53709d07 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -27,14 +27,6 @@
#include "move.h"
#include "fsm.h"
-#define MOVE_BORDER_END 500
-
-#define MOVE_ANGLE_0 0x0
-#define MOVE_ANGLE_90 0x4000
-#define MOVE_ANGLE_180 0x8000
-#define MOVE_ANGLE_270 0xC000
-
-
struct move_data_t move_data;
/** Start a move FSM. */
@@ -44,75 +36,9 @@ move_start (uint32_t position_x, uint32_t position_y)
/* Set parameters. */
move_data.position_x = position_x;
move_data.position_y = position_y;
+ move_data.nb_obstacle = 0;
/* Start the FSM. */
fsm_init (&move_fsm);
fsm_handle_event (&move_fsm, MOVE_EVENT_start);
}
-/** Verify if the position desired is in the table use when the robot tries to
- * reach a point and a obstacle is in front of it.
- * \param pos the robot's current position on the table.
- * \param new_pos the position desired by the user.
- * \return true if the it can reach the position.
- */
-uint8_t
-move_can_go_on_left_or_right (asserv_position_t current_pos,
- asserv_position_t new_pos)
-{
- // Go on right.
- if (new_pos.x > current_pos.x)
- {
- if (new_pos.x - current_pos.x < 3000 - MOVE_BORDER_END)
- return 0x1;
- else
- return 0x0;
- }
- // go on left.
- else
- {
- if (current_pos.x - new_pos.x < MOVE_BORDER_END)
- return 0x1;
- else
- return 0x0;
- }
-}
-
-/** Go to the right.
- */
-void
-move_go_to_right (void)
-{
- asserv_position_t pos;
- asserv_position_t new_pos;
-
- asserv_get_position (&pos);
- new_pos.x = pos.x - MOVE_BORDER_LEVEL;
- new_pos.y = pos.y;
- new_pos.a = pos.a;
-
- if (move_can_go_on_left_or_right (pos, new_pos))
- {
- asserv_goto (new_pos.x, new_pos.y);
- }
-}
-
-/** Go to the left.
- */
-void
-move_go_to_left (void)
-{
- asserv_position_t pos;
- asserv_position_t new_pos;
-
- asserv_get_position (&pos);
- new_pos.x = pos.x + MOVE_BORDER_LEVEL;
- new_pos.y = pos.y;
- new_pos.a = pos.a;
-
- if (move_can_go_on_left_or_right (pos, new_pos))
- {
- asserv_goto (new_pos.x, new_pos.y);
- }
-}
-
-
diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm
index 5e2f877b..9eaaf02b 100644
--- a/digital/io/src/move.fsm
+++ b/digital/io/src/move.fsm
@@ -4,34 +4,43 @@ move
States:
IDLE
DESIRED_POSITION
- MOVE_ON_LEFT
- MOVE_ON_RIGHT
+ BACKWARD
+ GO_AWAY
+ STOP_WAIT
Events:
start
- reached
- blocked
+ bot_move_succeed
+ bot_move_failed
+ bot_move_obstacle
+ wait_finished
IDLE:
start -> DESIRED_POSITION
- Tries to reach a position provided by the user. If the position desired can not be reached, it all try to move on the right or the left.
+ Save the destination position and put obstacles number to 0.
DESIRED_POSITION:
- reached -> IDLE
- The position provided by the user has been reached, the FSM can stop.
- blocked:near_left_border -> MOVE_ON_RIGHT
- The robot will compute the position from the border and will take the decision to go on the right because the left border is too near.
- blocked:near_right_border -> MOVE_ON_LEFT
- The robot will compute the position from the border and will take the decision to go on the left because the right border is too near.
+ 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.
-MOVE_ON_LEFT:
- reached -> DESIRED_POSITION
- The position has been reached. It will now try to reach the position provided by the user.
- blocked -> MOVE_ON_RIGHT
- The robot will try to go on the left because the previous movement block on the right.
+BACKWARD:
+ bot_move_succeed-> GO_AWAY
+ Launch the pathfinder and go to the next position.
-MOVE_ON_RIGHT:
- reached -> DESIRED_POSITION
- The position has been reached. It will now try to reach the position provided by the user.
- blocked -> MOVE_ON_LEFT
- The robot will try to go on the left because the previous movement block on the left.
+STOP_WAIT:
+ wait_finished-> GO_AWAY
+ Launch the pathfinder and go to the next position.
+
+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.
diff --git a/digital/io/src/move.h b/digital/io/src/move.h
index 69a2975e..b5d2a483 100644
--- a/digital/io/src/move.h
+++ b/digital/io/src/move.h
@@ -27,17 +27,24 @@
#include "asserv.h"
-/* if the left border is under 500 mm of the actual position do not go there. */
-#define MOVE_BORDER_LEVEL 200
-
+#define MOVE_OBSTACLE_RADIUS 100
+#define MOVE_OBSTACLE_VALIDITY 650
/** move FSM associated data. */
struct move_data_t
{
/* Desired x position to reached. */
- uint32_t position_x;
+ uint16_t position_x;
/* Desired y position to reached. */
- uint32_t position_y;
+ 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;
};
@@ -48,24 +55,4 @@ extern struct move_data_t move_data;
void
move_start (uint32_t position_x, uint32_t position_y);
-/** Verify if the position desired is in the table use when the robot tries to
- * reach a point and a obstacle is in front of it.
- * \param pos the robot's current position on the table.
- * \param new_pos the position computed to go to.
- * \return true if the it can reach the position.
- */
-uint8_t
-move_can_go_on_left_or_right (asserv_position_t current_pos,
- asserv_position_t new_pos);
-
-/** Go to the right.
- */
-void
-move_go_to_right (void);
-
-/** Go to the left.
- */
-void
-move_go_to_left (void);
-
#endif /* move_h */
diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c
index a54f9420..2c17927f 100644
--- a/digital/io/src/move_cb.c
+++ b/digital/io/src/move_cb.c
@@ -8,111 +8,173 @@
#include "common.h"
#include "fsm.h"
#include "move_cb.h"
-#include "move.h"
+#include "path.h"
#include "asserv.h"
-#include "main.h" /* main_post_event_for_top_fsm */
+#include "playground.h"
+#include "move.h"
/*
- * IDLE =start=>
- * => DESIRED_POSITION
- * Tries to reach a position provided by the user. If the position desired can not be reached, it all try to move on the right or the left.
+ * 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.
*/
fsm_branch_t
-move__IDLE__start (void)
+move__GO_AWAY__bot_move_succeed (void)
{
- asserv_goto (move_data.position_x, move_data.position_y);
- return move_next (IDLE, start);
+ 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 ();
+
+ 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);
+ }
}
/*
- * MOVE_ON_RIGHT =blocked=>
- * => MOVE_ON_LEFT
- * The robot will try to go on the left because the previous movement block on the left.
+ * GO_AWAY =bot_move_failed=>
+ * => BACKWARD
+ * Store the current position, and go backward.
*/
fsm_branch_t
-move__MOVE_ON_RIGHT__blocked (void)
+move__GO_AWAY__bot_move_failed (void)
{
- move_go_to_left ();
- return move_next (MOVE_ON_RIGHT, blocked);
+ asserv_move_linearly (-PG_MOVE_DISTANCE);
+ return move_next (GO_AWAY, bot_move_failed);
}
/*
- * MOVE_ON_RIGHT =reached=>
+ * GO_AWAY =bot_move_obstacle=>
+ * => STOP_WAIT
+ * Go to the stop wait event.
+ */
+fsm_branch_t
+move__GO_AWAY__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 ++;
+
+ // TODO: Set flag on main to wait.
+ return move_next (GO_AWAY, bot_move_obstacle);
+}
+
+/*
+ * STOP_WAIT =wait_finished=>
+ * => GO_AWAY
+ * Launch the pathfinder and go to the next position.
+ */
+fsm_branch_t
+move__STOP_WAIT__wait_finished (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);
+}
+
+/*
+ * IDLE =start=>
* => DESIRED_POSITION
- * The position has been reached. It will now try to reach the position provided by the user.
+ * Save the destination position and put obstacles number to 0.
*/
fsm_branch_t
-move__MOVE_ON_RIGHT__reached (void)
+move__IDLE__start (void)
{
+ // Try to go the desired position.
asserv_goto (move_data.position_x, move_data.position_y);
- return move_next (MOVE_ON_RIGHT, reached);
+ return move_next (IDLE, start);
}
/*
- * DESIRED_POSITION =blocked=>
- * near_right_border => MOVE_ON_LEFT
- * The robot will compute the position from the border and will take the decision to go on the left because the right border is too near.
- * near_left_border => MOVE_ON_RIGHT
- * The robot will compute the position from the border and will take the decision to go on the right because the left border is too near.
+ * BACKWARD =bot_move_succeed=>
+ * => GO_AWAY
+ * Launch the pathfinder and go to the next position.
*/
fsm_branch_t
-move__DESIRED_POSITION__blocked (void)
+move__BACKWARD__bot_move_succeed (void)
{
- asserv_position_t pos;
- asserv_position_t new_pos;
+ asserv_position_t current_pos;
- asserv_get_position (&pos);
- new_pos = pos;
- new_pos.x += MOVE_BORDER_LEVEL ;
+ // 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 ();
- if (move_can_go_on_left_or_right (pos, new_pos))
- {
- asserv_goto (new_pos.x, new_pos.y);
- return move_next_branch (DESIRED_POSITION, blocked, near_left_border);
- }
- else
- {
- new_pos.x = pos.x + MOVE_BORDER_LEVEL;
- // replace this by the correct function.
- asserv_goto (new_pos.x, new_pos.y);
- return move_next_branch (DESIRED_POSITION, blocked, near_right_border);
- }
+ // 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);
}
/*
- * DESIRED_POSITION =reached=>
- * => IDLE
- * The position provided by the user has been reached, the FSM can stop.
+ * 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.
*/
fsm_branch_t
-move__DESIRED_POSITION__reached (void)
+move__DESIRED_POSITION__bot_move_failed (void)
{
- main_post_event_for_top_fsm = TOP_EVENT_move_fsm_finished;
- return move_next (DESIRED_POSITION, reached);
+ // Go backward linearly.
+ asserv_move_linearly (-PG_MOVE_DISTANCE);
+
+ return move_next (DESIRED_POSITION, bot_move_failed);
}
/*
- * MOVE_ON_LEFT =blocked=>
- * => MOVE_ON_RIGHT
- * The robot will try to go on the left because the previous movement block on the right.
+ * DESIRED_POSITION =bot_move_succeed=>
+ * => IDLE
+ * Destination position reached by the bot. Set the flag of the top event to inform it.
*/
fsm_branch_t
-move__MOVE_ON_LEFT__blocked (void)
+move__DESIRED_POSITION__bot_move_succeed (void)
{
- move_go_to_right();
- return move_next (MOVE_ON_LEFT, blocked);
+ return move_next (DESIRED_POSITION, bot_move_succeed);
}
/*
- * MOVE_ON_LEFT =reached=>
- * => DESIRED_POSITION
- * The position has been reached. It will now try to reach the position provided by the user.
+ * DESIRED_POSITION =bot_move_obstacle=>
+ * => STOP_WAIT
+ * Stop the bot.
*/
fsm_branch_t
-move__MOVE_ON_LEFT__reached (void)
+move__DESIRED_POSITION__bot_move_obstacle (void)
{
- asserv_goto (move_data.position_x, move_data.position_y);
- return move_next (MOVE_ON_LEFT, reached);
-}
+ 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);
+}
diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h
index 4a3be101..ae97784e 100644
--- a/digital/io/src/playground.h
+++ b/digital/io/src/playground.h
@@ -114,4 +114,9 @@
#define PG_GUTTER_Y (300)
#define PG_GUTTER_A (90 * BOT_ANGLE_DEGREE)
+/**
+ * The backward move for the MOVE_FSM.
+ */
+#define PG_MOVE_DISTANCE 300
+
#endif // playground_h