summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJérémy Dufour2009-05-20 16:04:33 +0200
committerJérémy Dufour2009-05-20 16:04:33 +0200
commit73bbc9e206772e928878126187789448ab3d0d47 (patch)
tree4514afbe063ceb91e8f757752d2344b73e17683a
parent702c66479a3599758cd3537ea9c966e8f84a4ef0 (diff)
* digital/io/src:
- add direction support in goto command, - move FSM use the angle for the end of the movement.
-rw-r--r--digital/io/src/asserv.c4
-rw-r--r--digital/io/src/asserv.h3
-rw-r--r--digital/io/src/init_cb.c2
-rw-r--r--digital/io/src/move.c16
-rw-r--r--digital/io/src/move.h13
-rw-r--r--digital/io/src/move_cb.c55
6 files changed, 45 insertions, 48 deletions
diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c
index 3a22d0bc..062ca13a 100644
--- a/digital/io/src/asserv.c
+++ b/digital/io/src/asserv.c
@@ -460,7 +460,7 @@ asserv_goto_angle (int16_t angle)
/* Go to an absolute position and then an absolute angle. */
void
-asserv_goto_xya (uint32_t x, uint32_t y, int16_t a)
+asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward)
{
x = fixed_mul_f824 (x, asserv_scale_inv);
y = fixed_mul_f824 (y, asserv_scale_inv);
@@ -476,7 +476,7 @@ asserv_goto_xya (uint32_t x, uint32_t y, int16_t a)
asserv_twi_buffer_param[6] = v16_to_v8 (a, 1);
asserv_twi_buffer_param[7] = v16_to_v8 (a, 0);
/* No backward. */
- asserv_twi_buffer_param[8] = 0;
+ asserv_twi_buffer_param[8] = backward;
/* Send the got to an absolute position and them absolute angle command to
* the asserv board */
asserv_twi_send_command ('X', 9);
diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h
index 7e2ba988..82f405c0 100644
--- a/digital/io/src/asserv.h
+++ b/digital/io/src/asserv.h
@@ -226,9 +226,10 @@ asserv_goto_angle (int16_t angle);
* @param x the absolute position on the X axis.
* @param y the absolute position on the Y axis.
* @param a the absolute angle.
+ * @param backward 0 no backward, 1 backward possible, 2 backward compulsary.
*/
void
-asserv_goto_xya (uint32_t x, uint32_t y, int16_t a);
+asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward);
/**
* Go to the wall (moving backward).
diff --git a/digital/io/src/init_cb.c b/digital/io/src/init_cb.c
index 063314a3..b74937b2 100644
--- a/digital/io/src/init_cb.c
+++ b/digital/io/src/init_cb.c
@@ -211,7 +211,7 @@ init__GO_AWAY_FROM_THE_WALL_AGAIN__bot_move_succeed (void)
- BOT_WIDTH / 2
- 50),
PG_LENGTH - PG_START_ZONE_LENGTH + BOT_LENGTH / 2 + 50,
- PG_A_VALUE_COMPUTING (0 * BOT_ANGLE_DEGREE));
+ PG_A_VALUE_COMPUTING (0 * BOT_ANGLE_DEGREE), 0);
return init_next (GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed);
}
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index c30e5ace..7acc6c8b 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -34,20 +34,14 @@
*/
struct move_data_t move_data;
-/**
- * Go to a position with the start FSM.
- * @param position_x the X position.
- * @param position_y the Y position.
- * @param backward_movement_allowed do we allow backward movement?
- */
void
-move_start (uint16_t position_x, uint16_t position_y, uint8_t
- backward_movement_allowed)
+move_start (asserv_position_t position, uint8_t backward)
{
/* Set parameters. */
- move_data.final.x = position_x;
- move_data.final.y = position_y;
- move_data.backward_movement_allowed = backward_movement_allowed;
+ move_data.final.x = position.x;
+ move_data.final.y = position.y;
+ move_data.final.a = position.a;
+ move_data.backward_movement_allowed = backward;
/* XXX */
main_always_stop_for_obstacle = 1;
/* Reset move FSM flags */
diff --git a/digital/io/src/move.h b/digital/io/src/move.h
index 1b1621af..40fadf30 100644
--- a/digital/io/src/move.h
+++ b/digital/io/src/move.h
@@ -25,6 +25,8 @@
*
* }}} */
+#include "asserv.h"
+
/**
* A position.
*/
@@ -34,6 +36,8 @@ typedef struct move_position_t
uint16_t x;
/** Y position. */
uint16_t y;
+ /** A angle. */
+ uint16_t a;
} move_position_t;
/**
@@ -60,12 +64,11 @@ extern struct move_data_t move_data;
/**
* Go to a position with the start FSM.
- * @param position_x the X position.
- * @param position_y the Y position.
- * @param backward_movement_allowed do we allow backward movement?
+ * @param position the destination position.
+ * @param backward_movement_allowed 0, no backward, 1 backward is allowed, 2
+ * backward is compulsary.
*/
void
-move_start (uint16_t position_x, uint16_t position_y, uint8_t
- backward_movement_allowed);
+move_start (asserv_position_t position, uint8_t backward);
#endif /* move_h */
diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c
index a5622369..98f609b6 100644
--- a/digital/io/src/move_cb.c
+++ b/digital/io/src/move_cb.c
@@ -243,10 +243,10 @@ move_after_moving_backward (void)
if (move_get_next_position (&move_data.intermediate))
{
/* Go to the next position */
- if (move_data.backward_movement_allowed)
- asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y);
- else
- asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ asserv_goto_xya (move_data.intermediate.x,
+ move_data.intermediate.y,
+ move_data.intermediate.a,
+ move_data.backward_movement_allowed);
return 1;
}
else
@@ -273,10 +273,9 @@ move__WAIT_FOR_CLEAR_PATH__wait_finished (void)
if (!sharp_path_obstrued (1))
{
/* Try to go to the final position */
- if (move_data.backward_movement_allowed)
- asserv_goto_back (move_data.final.x, move_data.final.y);
- else
- asserv_goto (move_data.final.x, move_data.final.y);
+ asserv_goto_xya (move_data.final.x, move_data.final.y,
+ move_data.final.a,
+ move_data.backward_movement_allowed);
return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, no_obstacle);
}
else
@@ -289,10 +288,10 @@ 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 */
- if (move_data.backward_movement_allowed)
- asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y);
- else
- asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ asserv_goto_xya (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
@@ -317,10 +316,10 @@ fsm_branch_t
move__IDLE__start (void)
{
/* Go to the destination position */
- if (move_data.backward_movement_allowed)
- asserv_goto_back (move_data.final.x, move_data.final.y);
- else
- asserv_goto (move_data.final.x, move_data.final.y);
+ 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);
}
@@ -351,10 +350,10 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_succeed (void)
if (move_get_next_position (&move_data.intermediate))
{
/* Go to the next intermediate position */
- if (move_data.backward_movement_allowed)
- asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y);
- else
- asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ asserv_goto_xya (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
@@ -392,10 +391,10 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_obstacle (void)
if (move_get_next_position (&move_data.intermediate))
{
/* Go to the next intermediate position */
- if (move_data.backward_movement_allowed)
- asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y);
- else
- asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ asserv_goto_xya (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_obstacle, intermediate_path_found);
}
else
@@ -539,10 +538,10 @@ move__MOVING_TO_FINAL_POSITION__bot_move_obstacle (void)
if (move_get_next_position (&move_data.intermediate))
{
/* Go to the next intermediate position */
- if (move_data.backward_movement_allowed)
- asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y);
- else
- asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ asserv_goto_xya (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);
}
else