summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-14 00:52:44 +0200
committerNicolas Schodet2010-05-14 00:52:44 +0200
commit3966014502a626b938c473535935748793c3f904 (patch)
tree99c04cdcf7c34bc56e041bbae883e455c543b19b
parent1b290a3f5f6f084d5e2f142d6cea4d47fc36cf75 (diff)
digital/io/src: control rotation in move FSM
-rw-r--r--digital/io/src/ai_move_cb.c144
-rw-r--r--digital/io/src/move.fsm18
-rw-r--r--digital/io/src/move.h6
3 files changed, 139 insertions, 29 deletions
diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c
index 33f2939c..af2788ca 100644
--- a/digital/io/src/ai_move_cb.c
+++ b/digital/io/src/ai_move_cb.c
@@ -44,41 +44,80 @@
#include "debug.host.h"
+#include <math.h>
+
+/** Go or rotate toward position, returns 1 for linear move, 2 for angular
+ * move. */
+static uint8_t
+move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle,
+ uint8_t backward)
+{
+ position_t robot_position;
+ /* Remember step. */
+ move_data.step = dst;
+ move_data.step_angle = angle;
+ move_data.step_with_angle = with_angle;
+ move_data.step_backward = backward;
+ /* Compute angle to destination. */
+ asserv_get_position (&robot_position);
+ vect_t v = dst; vect_sub (&v, &robot_position.v);
+ uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI));
+ if (backward & ASSERV_BACKWARD)
+ dst_angle += 0x8000;
+ if ((backward & ASSERV_REVERT_OK)
+ && (dst_angle ^ robot_position.a) & 0x8000)
+ dst_angle += 0x8000;
+ int16_t diff = dst_angle - robot_position.a;
+ /* Move or rotate. */
+ if (UTILS_ABS (diff) < 0x1000)
+ {
+ if (with_angle)
+ asserv_goto_xya (dst.x, dst.y, angle, backward);
+ else
+ asserv_goto (dst.x, dst.y, backward);
+ return 1;
+ }
+ else
+ {
+ asserv_goto_angle (dst_angle);
+ return 2;
+ }
+}
+
/** Go to next position computed by path module, to be called by
- * move_path_init and move_path_next. */
-static void
+ * move_path_init and move_path_next. Returns 1 for linear move, 2 for angular
+ * move. */
+static uint8_t
move_go_to_next (vect_t dst)
{
+ uint8_t r;
/* If it is not the last position. */
if (dst.x != move_data.final.v.x || dst.y != move_data.final.v.y)
{
/* Not final position. */
move_data.final_move = 0;
/* Goto without angle. */
- asserv_goto (dst.x, dst.y, move_data.backward_movement_allowed
- | (move_data.slow ? ASSERV_REVERT_OK : 0));
+ r = move_go_or_rotate (dst, 0, 0, move_data.backward_movement_allowed
+ | (move_data.slow ? ASSERV_REVERT_OK : 0));
}
else
{
/* Final position. */
move_data.final_move = 1;
/* 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);
+ r = move_go_or_rotate (dst, move_data.final.a, move_data.with_angle,
+ move_data.backward_movement_allowed);
}
- move_data.step = dst;
TRACE (TRACE_MOVE__GO_TO, dst.x, dst.y);
/* Reset try counter. */
move_data.try_again_counter = 3;
/* Next time, do not use slow. */
move_data.slow = 0;
+ return r;
}
-/** Update and go to first position, return non zero if a path is found. */
+/** Update and go to first position, return non zero if a path is found, 1 for
+ * linear move, 2 for angular move. */
static uint8_t
move_path_init (void)
{
@@ -104,8 +143,7 @@ move_path_init (void)
/* If found, go. */
if (found)
{
- move_go_to_next (dst);
- return 1;
+ return move_go_to_next (dst);
}
else
{
@@ -115,17 +153,20 @@ move_path_init (void)
}
}
-/** Go to next position in path. */
-static void
+/** Go to next position in path. Returns 1 for linear move, 2 for angular
+ * move. */
+static uint8_t
move_path_next (void)
{
vect_t dst;
path_get_next (&dst);
- move_go_to_next (dst);
+ return move_go_to_next (dst);
}
/*
* MOVE_IDLE =move_start=>
+ * path_found_rotate => MOVE_ROTATING
+ * rotate towards next position.
* path_found => MOVE_MOVING
* move to next position.
* no_path_found => MOVE_IDLE
@@ -134,8 +175,14 @@ move_path_next (void)
fsm_branch_t
ai__MOVE_IDLE__move_start (void)
{
- if (move_path_init ())
- return ai_next_branch (MOVE_IDLE, move_start, path_found);
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return ai_next_branch (MOVE_IDLE, move_start, path_found_rotate);
+ else
+ return ai_next_branch (MOVE_IDLE, move_start, path_found);
+ }
else
{
main_post_event (AI_EVENT_move_fsm_failed);
@@ -144,9 +191,28 @@ ai__MOVE_IDLE__move_start (void)
}
/*
+ * MOVE_ROTATING =bot_move_succeed=>
+ * => MOVE_MOVING
+ * move to next position.
+ */
+fsm_branch_t
+ai__MOVE_ROTATING__bot_move_succeed (void)
+{
+ if (move_data.step_with_angle)
+ asserv_goto_xya (move_data.step.x, move_data.step.y,
+ move_data.step_angle, move_data.step_backward);
+ else
+ asserv_goto (move_data.step.x, move_data.step.y,
+ move_data.step_backward);
+ return ai_next (MOVE_ROTATING, bot_move_succeed);
+}
+
+/*
* MOVE_MOVING =bot_move_succeed=>
* done => MOVE_IDLE
* post success event.
+ * path_found_rotate => MOVE_ROTATING
+ * rotate towards next position.
* path_found => MOVE_MOVING
* move to next position.
* no_path_found => MOVE_IDLE
@@ -162,8 +228,11 @@ ai__MOVE_MOVING__bot_move_succeed (void)
}
else
{
- move_path_next ();
- return ai_next_branch (MOVE_MOVING, bot_move_succeed, path_found);
+ uint8_t next = move_path_next ();
+ if (next == 2)
+ return ai_next_branch (MOVE_MOVING, bot_move_succeed, path_found_rotate);
+ else
+ return ai_next_branch (MOVE_MOVING, bot_move_succeed, path_found);
}
//return ai_next_branch (MOVE_MOVING, bot_move_succeed, no_path_found);
}
@@ -211,6 +280,8 @@ ai__MOVE_MOVING__obstacle_in_front (void)
/*
* MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=>
+ * path_found_rotate => MOVE_ROTATING
+ * rotate towards next position.
* path_found => MOVE_MOVING
* move to next position.
* no_path_found => MOVE_IDLE
@@ -219,9 +290,13 @@ ai__MOVE_MOVING__obstacle_in_front (void)
fsm_branch_t
ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void)
{
- if (move_path_init ())
+ uint8_t next = move_path_init ();
+ if (next)
{
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found);
+ if (next == 2)
+ return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found_rotate);
+ else
+ return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_succeed, path_found);
}
else
{
@@ -232,6 +307,8 @@ ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void)
/*
* MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_failed=>
+ * path_found_rotate => MOVE_ROTATING
+ * rotate towards next position.
* path_found => MOVE_MOVING
* move to next position.
* no_path_found => MOVE_WAIT_FOR_CLEAR_PATH
@@ -240,14 +317,22 @@ ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void)
fsm_branch_t
ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void)
{
- if (move_path_init ())
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found);
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found_rotate);
+ else
+ return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found);
+ }
else
return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found);
}
/*
* MOVE_WAIT_FOR_CLEAR_PATH =state_timeout=>
+ * path_found_rotate => MOVE_ROTATING
+ * rotate towards next position.
* path_found => MOVE_MOVING
* move to next position.
* no_path_found_and_try_again => MOVE_WAIT_FOR_CLEAR_PATH
@@ -259,10 +344,13 @@ fsm_branch_t
ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void)
{
/* Try to move. */
- if (move_path_init ())
+ uint8_t next = move_path_init ();
+ if (next)
{
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout,
- path_found);
+ if (next == 2)
+ return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, path_found_rotate);
+ else
+ return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, path_found);
}
else
{
diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm
index d96cb349..20334686 100644
--- a/digital/io/src/move.fsm
+++ b/digital/io/src/move.fsm
@@ -8,6 +8,8 @@ move
States:
*MOVE_IDLE
waiting for the beginning of the move FSM.
+ MOVE_ROTATING
+ rotating towards next point.
MOVE_MOVING
moving to a position (intermediate or final).
MOVE_MOVING_BACKWARD_TO_TURN_FREELY
@@ -22,15 +24,23 @@ Events:
the bot has seen something (front is the same when going backward).
MOVE_IDLE:
+ move_start: path_found_rotate -> MOVE_ROTATING
+ rotate towards next position.
move_start: path_found -> MOVE_MOVING
move to next position.
move_start: no_path_found -> MOVE_IDLE
post failure event.
+MOVE_ROTATING:
+ bot_move_succeed -> MOVE_MOVING
+ move to next position.
+
MOVE_MOVING:
bot_move_succeed: done -> MOVE_IDLE
post success event.
- bot_move_succeed: path_found -> .
+ bot_move_succeed: path_found_rotate -> MOVE_ROTATING
+ rotate towards next position.
+ bot_move_succeed: path_found -> MOVE_MOVING
move to next position.
bot_move_succeed: no_path_found -> MOVE_IDLE
post failure event.
@@ -42,16 +52,22 @@ MOVE_MOVING:
stop the bot.
MOVE_MOVING_BACKWARD_TO_TURN_FREELY:
+ bot_move_succeed: path_found_rotate -> MOVE_ROTATING
+ rotate towards next position.
bot_move_succeed: path_found -> MOVE_MOVING
move to next position.
bot_move_succeed: no_path_found -> MOVE_IDLE
post failure event.
+ bot_move_failed: path_found_rotate -> MOVE_ROTATING
+ rotate towards next position.
bot_move_failed: path_found -> MOVE_MOVING
move to next position.
bot_move_failed: no_path_found -> MOVE_WAIT_FOR_CLEAR_PATH
nothing to do.
MOVE_WAIT_FOR_CLEAR_PATH:
+ state_timeout: path_found_rotate -> MOVE_ROTATING
+ rotate towards next position.
state_timeout: path_found -> MOVE_MOVING
move to next position.
state_timeout: no_path_found_and_try_again -> .
diff --git a/digital/io/src/move.h b/digital/io/src/move.h
index fa252e4c..7e52c6b9 100644
--- a/digital/io/src/move.h
+++ b/digital/io/src/move.h
@@ -51,6 +51,12 @@ struct move_data_t
uint8_t with_angle;
/** Next step. */
vect_t step;
+ /** Next step angle. */
+ uint16_t step_angle;
+ /** Next step with_angle. */
+ uint8_t step_with_angle;
+ /** Next step backward. */
+ uint8_t step_backward;
/** Non zero means this is a tricky move, slow down, and minimize
* turns. */
uint8_t slow;