summaryrefslogtreecommitdiffhomepage
path: root/digital/io
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-13 00:46:21 +0200
committerNicolas Schodet2010-05-13 00:46:21 +0200
commit953ff53ccf92e700c7c25136eabbe6699caa90f9 (patch)
treeae5bcb08266c5c604297d392adce8050ac9cb164 /digital/io
parent3858cf3814d073a279309acbc587648bb10609b9 (diff)
digital/io/src: make angle optional in move FSM
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/src/ai_move_cb.c8
-rw-r--r--digital/io/src/ai_top_cb.c12
-rw-r--r--digital/io/src/move.c15
-rw-r--r--digital/io/src/move.h6
-rw-r--r--digital/io/src/playground.h4
5 files changed, 34 insertions, 11 deletions
diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c
index 7a8ca9cd..eaa0aead 100644
--- a/digital/io/src/ai_move_cb.c
+++ b/digital/io/src/ai_move_cb.c
@@ -88,8 +88,12 @@ move_get_next_position (void)
{
/* Final position. */
move_data.final_move = 1;
- /* Goto with angle. */
- asserv_goto_xya (dst.x, dst.y, move_data.final.a,
+ /* Goto with angle if requested. */
+ if (move_data.with_angle)
+ asserv_goto_xya (dst.x, dst.y, move_data.final.a,
+ move_data.backward_movement_allowed);
+ else
+ asserv_goto (dst.x, dst.y,
move_data.backward_movement_allowed);
}
move_data.step = dst;
diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c
index 745e0b6a..c057bebf 100644
--- a/digital/io/src/ai_top_cb.c
+++ b/digital/io/src/ai_top_cb.c
@@ -61,8 +61,7 @@ fsm_branch_t
ai__WAIT_INIT_TO_FINISH__init_match_is_started (void)
{
asserv_set_speed (BOT_MOVE_FAST);
- position_t pos = PG_POSITION_DEG (375, 1503, -29);
- move_start (pos, 0);
+ move_start_noangle (PG_VECT (375, 1503), 0);
return ai_next (WAIT_INIT_TO_FINISH, init_match_is_started);
}
@@ -75,8 +74,7 @@ ai__WAIT_INIT_TO_FINISH__init_match_is_started (void)
fsm_branch_t
ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_succeed (void)
{
- position_t pos = PG_POSITION_DEG (2625, 253, -29);
- move_start (pos, 0);
+ move_start_noangle (PG_VECT (2625, 253), 0);
loader_down ();
return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_succeed);
}
@@ -91,8 +89,7 @@ fsm_branch_t
ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_failed (void)
{
asserv_set_speed (BOT_MOVE_SLOW);
- position_t pos = PG_POSITION_DEG (375, 1503, -29);
- move_start (pos, 0);
+ move_start_noangle (PG_VECT (375, 1503), 0);
return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_failed);
}
@@ -156,8 +153,7 @@ ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_succeed (void)
fsm_branch_t
ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed (void)
{
- position_t pos = PG_POSITION_DEG (2625, 253, -29);
- move_start (pos, 0);
+ move_start_noangle (PG_VECT (2625, 253), 0);
return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed);
}
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index 24b8848d..77bbcf39 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -42,7 +42,20 @@ void
move_start (position_t position, uint8_t backward)
{
/* Set parameters. */
- move_data.final= position;
+ move_data.final = position;
+ move_data.with_angle = 1;
+ move_data.backward_movement_allowed = backward;
+ move_data.final_move = 0;
+ /* Start the FSM. */
+ fsm_handle_event (&ai_fsm, AI_EVENT_move_start);
+}
+
+void
+move_start_noangle (vect_t position, uint8_t backward)
+{
+ /* Set parameters. */
+ move_data.final.v = position;
+ move_data.with_angle = 0;
move_data.backward_movement_allowed = backward;
move_data.final_move = 0;
/* Start the FSM. */
diff --git a/digital/io/src/move.h b/digital/io/src/move.h
index c504a0b2..214fddba 100644
--- a/digital/io/src/move.h
+++ b/digital/io/src/move.h
@@ -47,6 +47,8 @@ struct move_data_t
{
/** Final position. */
position_t final;
+ /** Use angle consign for final point. */
+ uint8_t with_angle;
/** Next step. */
vect_t step;
/** Backward direction allowed flag. */
@@ -71,6 +73,10 @@ extern struct move_data_t move_data;
void
move_start (position_t position, uint8_t backward);
+/** Go to a position, with no angle consign. */
+void
+move_start_noangle (vect_t position, uint8_t backward);
+
/** To be called when obstacles positions are computed. */
void
move_obstacles_update (void);
diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h
index 68e17ba0..5c251ce5 100644
--- a/digital/io/src/playground.h
+++ b/digital/io/src/playground.h
@@ -76,6 +76,10 @@
#define PG_POSITION_DEG(x, y, a) \
{ { PG_X (x), PG_Y (y) }, PG_A_DEG (a) }
+/** Initialiser for vect_t applying symmetry according to color. */
+#define PG_VECT(x, y) \
+ (vect_t) { PG_X (x), PG_Y (y) }
+
/**
* Start zone.
*/