summaryrefslogtreecommitdiff
path: root/digital/io/src/move_cb.c
diff options
context:
space:
mode:
authorJérémy Dufour2009-05-20 19:22:02 +0200
committerJérémy Dufour2009-05-20 19:22:02 +0200
commit503e3511b2fa42263749c62a9625e6549ac2fe2d (patch)
tree86826db066cceaf8a9351f0f0adb11327ed8a3f5 /digital/io/src/move_cb.c
parentfbb64cf43457a96dc7757f649d9f2dc3d6243d19 (diff)
* digital/io/src:
- support backward parameter for the asserv_goto, - enhance to avoid static obstacle, - add center circle of the playground table.
Diffstat (limited to 'digital/io/src/move_cb.c')
-rw-r--r--digital/io/src/move_cb.c121
1 files changed, 90 insertions, 31 deletions
diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c
index 98f609b6..95dc3df9 100644
--- a/digital/io/src/move_cb.c
+++ b/digital/io/src/move_cb.c
@@ -243,10 +243,18 @@ move_after_moving_backward (void)
if (move_get_next_position (&move_data.intermediate))
{
/* Go to the next position */
- asserv_goto_xya (move_data.intermediate.x,
+ if (move_data.intermediate.x == move_data.final.x
+ && move_data.intermediate.y == move_data.final.y)
+ asserv_goto_xya (move_data.intermediate.x,
+ move_data.intermediate.y,
+ move_data.final.a,
+ move_data.backward_movement_allowed);
+ else
+ asserv_goto (move_data.intermediate.x,
move_data.intermediate.y,
- move_data.intermediate.a,
move_data.backward_movement_allowed);
+
+
return 1;
}
else
@@ -254,6 +262,46 @@ move_after_moving_backward (void)
}
/*
+ * IDLE =start=>
+ * intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION
+ * ask the asserv to go to the intermediate position
+ * no_intermediate_path_found => MOVING_TO_FINAL_POSITION
+ * ask the asserv to go to the final position
+ */
+fsm_branch_t
+move__IDLE__start (void)
+{
+ if (!move_get_next_position (&move_data.intermediate))
+ {
+ if (move_data.intermediate.x == move_data.final.x
+ && move_data.intermediate.y == move_data.final.y)
+ {
+ /* Go to the destination position */
+ asserv_goto_xya (move_data.final.x,
+ move_data.final.y,
+ move_data.final.a,
+ move_data.backward_movement_allowed);
+ return move_next_branch (IDLE, start, no_intermediate_path_found);
+ }
+ else
+ {
+ asserv_goto (move_data.intermediate.x,
+ move_data.intermediate.y,
+ move_data.backward_movement_allowed);
+ return move_next_branch (IDLE, start, intermediate_path_found);
+ }
+ }
+ else
+ {
+ asserv_goto_xya (move_data.final.x,
+ move_data.final.y,
+ move_data.final.a,
+ move_data.backward_movement_allowed);
+ return move_next_branch (IDLE, start, no_intermediate_path_found);
+ }
+}
+
+/*
* WAIT_FOR_CLEAR_PATH =wait_finished=>
* no_obstacle => MOVING_TO_FINAL_POSITION
* ask the asserv to go to the final position
@@ -287,11 +335,18 @@ move__WAIT_FOR_CLEAR_PATH__wait_finished (void)
{
/* Ignore sharps */
main_sharp_ignore_event = MOVE_MAIN_IGNORE_SHARP_EVENT;
- /* Go to the next intermediate position */
- asserv_goto_xya (move_data.intermediate.x,
+ if (move_data.intermediate.x == move_data.final.x
+ && move_data.intermediate.y == move_data.final.y)
+ /* Go to the next intermediate position */
+ asserv_goto_xya (move_data.intermediate.x,
+ move_data.intermediate.y,
+ move_data.final.a,
+ move_data.backward_movement_allowed);
+ else
+ asserv_goto (move_data.intermediate.x,
move_data.intermediate.y,
- move_data.intermediate.a,
move_data.backward_movement_allowed);
+
return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle_and_intermediate_path_found);
}
else
@@ -307,21 +362,6 @@ move__WAIT_FOR_CLEAR_PATH__wait_finished (void)
}
}
-/*
- * IDLE =start=>
- * => MOVING_TO_FINAL_POSITION
- * ask the asserv to go to the final position
- */
-fsm_branch_t
-move__IDLE__start (void)
-{
- /* Go to the destination position */
- asserv_goto_xya (move_data.final.x,
- move_data.final.y,
- move_data.final.a,
- move_data.backward_movement_allowed);
- return move_next (IDLE, start);
-}
/*
* MOVING_TO_INTERMEDIATE_POSITION =bot_move_succeed=>
@@ -349,11 +389,18 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_succeed (void)
/* Get next position */
if (move_get_next_position (&move_data.intermediate))
{
- /* Go to the next intermediate position */
- asserv_goto_xya (move_data.intermediate.x,
+ if (move_data.intermediate.x == move_data.final.x
+ && move_data.intermediate.y == move_data.final.y)
+ /* Go to the next intermediate position */
+ asserv_goto_xya (move_data.intermediate.x,
+ move_data.intermediate.y,
+ move_data.final.a,
+ move_data.backward_movement_allowed);
+ else
+ asserv_goto (move_data.intermediate.x,
move_data.intermediate.y,
- move_data.intermediate.a,
move_data.backward_movement_allowed);
+
return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, position_intermediary);
}
else
@@ -390,11 +437,17 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_obstacle (void)
/* Get next position */
if (move_get_next_position (&move_data.intermediate))
{
- /* Go to the next intermediate position */
- asserv_goto_xya (move_data.intermediate.x,
- move_data.intermediate.y,
- move_data.intermediate.a,
- move_data.backward_movement_allowed);
+ if (move_data.intermediate.x == move_data.final.x
+ && move_data.intermediate.y == move_data.final.y)
+ /* Go to the next intermediate position */
+ asserv_goto_xya (move_data.intermediate.x,
+ move_data.intermediate.y,
+ move_data.final.a,
+ move_data.backward_movement_allowed);
+ else
+ asserv_goto (move_data.intermediate.x,
+ move_data.intermediate.y,
+ move_data.backward_movement_allowed);
return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_obstacle, intermediate_path_found);
}
else
@@ -537,10 +590,16 @@ move__MOVING_TO_FINAL_POSITION__bot_move_obstacle (void)
/* Get next position */
if (move_get_next_position (&move_data.intermediate))
{
- /* Go to the next intermediate position */
- asserv_goto_xya (move_data.intermediate.x,
+ if (move_data.intermediate.x == move_data.final.x
+ && move_data.intermediate.y == move_data.final.y)
+ /* Go to the next intermediate position */
+ asserv_goto_xya (move_data.intermediate.x,
+ move_data.intermediate.y,
+ move_data.final.a,
+ move_data.backward_movement_allowed);
+ else
+ asserv_goto (move_data.intermediate.x,
move_data.intermediate.y,
- move_data.intermediate.a,
move_data.backward_movement_allowed);
return move_next_branch (MOVING_TO_FINAL_POSITION, bot_move_obstacle, intermediate_path_found);
}