summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/move_cb.c
diff options
context:
space:
mode:
authorJérémy Dufour2008-05-03 12:43:19 +0200
committerJérémy Dufour2008-05-03 12:43:19 +0200
commit23bce6a75a17b9cd9d3f35a251baa1b0635697a4 (patch)
treecc53e275464e9e86cf6fe705497d0212fea960f1 /digital/io/src/move_cb.c
parent1bcff7dbcbb284f5564ac708292b23fd447395a9 (diff)
* digital/io/src
- manage the event where the bot can be blocked (when moving) in the different FSM, by waiting and trying again ; - ensure we update the classifier (with the servos motor) even when the arm move in a state where it should not.
Diffstat (limited to 'digital/io/src/move_cb.c')
-rw-r--r--digital/io/src/move_cb.c16
1 files changed, 11 insertions, 5 deletions
diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c
index e85a4ef7..c55bfa30 100644
--- a/digital/io/src/move_cb.c
+++ b/digital/io/src/move_cb.c
@@ -157,7 +157,7 @@ move_compute_obstacle_position (asserv_position_t cur,
/* Convert the angle */
uint32_t angle = cur.a;
/* Change angle when going backward */
- if (asserv_get_moving_direction () == 2)
+ if (0)
angle += 0x8000;
angle = angle << 8;
DPRINTF ("We are at (%d ; %d ; %x)\n", cur.x, cur.y, cur.a);
@@ -236,7 +236,8 @@ move_after_moving_backward (void)
fsm_branch_t
move__WAIT_FOR_CLEAR_PATH__wait_finished (void)
{
- if (!sharp_path_obstrued (move_data.cached_moving_direction))
+/// if (!sharp_path_obstrued (move_data.cached_moving_direction))
+ if (!sharp_path_obstrued (1))
{
/* Try to go to the final position */
if (move_data.backward_movement_allowed)
@@ -264,12 +265,12 @@ move__WAIT_FOR_CLEAR_PATH__wait_finished (void)
else
{
/* Store current moving direction */
- move_data.cached_moving_direction = asserv_get_moving_direction ();
+ move_data.cached_moving_direction = asserv_get_moving_direction ();
/* Stop the bot */
asserv_stop_motor ();
/* Post an event for the top FSM to be waked up later */
main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP;
- return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle_and_intermediate_path_found);
+ return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle_and_no_intermediate_path_found);
}
}
}
@@ -411,6 +412,8 @@ move__MOVING_BACKWARD__bot_move_failed (void)
}
else
{
+ /* Store current moving direction */
+ move_data.cached_moving_direction = 2;
/* Stop the bot */
asserv_stop_motor ();
/* Post an event for the top FSM to be waked up later */
@@ -438,6 +441,8 @@ move__MOVING_BACKWARD__bot_move_succeed (void)
}
else
{
+ /* Store current moving direction */
+ move_data.cached_moving_direction = asserv_get_moving_direction ();
/* Stop the bot */
asserv_stop_motor ();
/* Post an event for the top FSM to be waked up later */
@@ -459,7 +464,8 @@ move__MOVING_TO_FINAL_POSITION__bot_move_failed (void)
/* Compute obstacle position */
move_obstacle_here ();
/* Store current moving direction */
- move_data.cached_moving_direction = asserv_get_moving_direction ();
+ move_data.cached_moving_direction = move_data.backward_movement_allowed ?
+ 2 : 1;
/* Move backward to turn freely */
asserv_move_linearly (-PG_MOVE_DISTANCE);
return move_next (MOVING_TO_FINAL_POSITION, bot_move_failed);