From d7211fc08b077030cf7f065d3834e201d17c18a5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 13 Apr 2010 00:21:48 +0200 Subject: digital/io/src: refactor position and angle macros --- digital/io/src/ai_init_cb.c | 14 ++++++-------- digital/io/src/ai_top_cb.c | 15 +++------------ digital/io/src/bot.h | 6 ------ digital/io/src/defs.h | 3 +++ digital/io/src/playground.h | 19 +++++++++++++++---- 5 files changed, 27 insertions(+), 30 deletions(-) (limited to 'digital') diff --git a/digital/io/src/ai_init_cb.c b/digital/io/src/ai_init_cb.c index a1d61967..1fb41b64 100644 --- a/digital/io/src/ai_init_cb.c +++ b/digital/io/src/ai_init_cb.c @@ -123,7 +123,7 @@ fsm_branch_t ai__INIT_SET_Y_POSITION__asserv_last_cmd_ack (void) { /* We are facing top border. */ - asserv_set_angle_position (90 * BOT_ANGLE_DEGREE); + asserv_set_angle_position (POSITION_A_DEG (90)); return ai_next (INIT_SET_Y_POSITION, asserv_last_cmd_ack); } @@ -149,7 +149,7 @@ fsm_branch_t ai__INIT_GO_AWAY_FROM_THE_WALL__bot_move_succeed (void) { /* Face the other wall. */ - asserv_goto_angle (PG_A_VALUE_COMPUTING (180 * BOT_ANGLE_DEGREE)); + asserv_goto_angle (PG_A_DEG (180)); return ai_next (INIT_GO_AWAY_FROM_THE_WALL, bot_move_succeed); } @@ -185,7 +185,7 @@ ai__INIT_WAIT_AFTER_ROTATION__state_timeout (void) fsm_branch_t ai__INIT_GO_TO_THE_WALL_AGAIN__bot_move_succeed (void) { - asserv_set_x_position (PG_X_VALUE_COMPUTING (BOT_LENGTH / 2)); + asserv_set_x_position (PG_X (BOT_LENGTH / 2)); return ai_next (INIT_GO_TO_THE_WALL_AGAIN, bot_move_succeed); } @@ -211,11 +211,9 @@ fsm_branch_t ai__INIT_GO_AWAY_FROM_THE_WALL_AGAIN__bot_move_succeed (void) { /* Move away from the border. */ - asserv_goto_xya (PG_X_VALUE_COMPUTING (PG_START_ZONE_WIDTH - - BOT_WIDTH / 2 - - 50), - PG_LENGTH - PG_START_ZONE_LENGTH + BOT_LENGTH / 2 + 50, - PG_A_VALUE_COMPUTING (0 * BOT_ANGLE_DEGREE), 0); + asserv_goto_xya (PG_X (PG_START_ZONE_WIDTH - BOT_WIDTH / 2 - 50), + PG_Y (PG_LENGTH - PG_START_ZONE_LENGTH + BOT_LENGTH / 2 + 50), + PG_A_DEG (0), 0); return ai_next (INIT_GO_AWAY_FROM_THE_WALL_AGAIN, bot_move_succeed); } diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c index 91937ad2..60f2c148 100644 --- a/digital/io/src/ai_top_cb.c +++ b/digital/io/src/ai_top_cb.c @@ -58,10 +58,7 @@ ai__IDLE__start (void) fsm_branch_t ai__WAIT_INIT_TO_FINISH__init_match_is_started (void) { - position_t pos; - pos.v.x = PG_X_VALUE_COMPUTING (2000); - pos.v.y = 1000; - pos.a = PG_A_VALUE_COMPUTING (0); + position_t pos = PG_POSITION_DEG (2000, 1000, 0); move_start (pos, 0); return ai_next (WAIT_INIT_TO_FINISH, init_match_is_started); } @@ -74,10 +71,7 @@ ai__WAIT_INIT_TO_FINISH__init_match_is_started (void) fsm_branch_t ai__GO_FAR__move_fsm_succeed (void) { - position_t pos; - pos.v.x = PG_X_VALUE_COMPUTING (1000); - pos.v.y = 1000; - pos.a = PG_A_VALUE_COMPUTING (0); + position_t pos = PG_POSITION_DEG (1000, 1000, 0); move_start (pos, 0); return ai_next (GO_FAR, move_fsm_succeed); } @@ -90,10 +84,7 @@ ai__GO_FAR__move_fsm_succeed (void) fsm_branch_t ai__GO_NEAR__move_fsm_succeed (void) { - position_t pos; - pos.v.x = PG_X_VALUE_COMPUTING (2000); - pos.v.y = 1000; - pos.a = PG_A_VALUE_COMPUTING (0); + position_t pos = PG_POSITION_DEG (2000, 1000, 0); move_start (pos, 0); return ai_next (GO_NEAR, move_fsm_succeed); } diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h index aff6ada0..78e9a51f 100644 --- a/digital/io/src/bot.h +++ b/digital/io/src/bot.h @@ -34,12 +34,6 @@ */ #define MATCH_DURATION_MS 90000 -/** - * How to compute a angle for the robot? - * One degree is 65536 / 360 - */ -#define BOT_ANGLE_DEGREE (65536 / 360) - /** * The scaling factor, millimeter per step. */ diff --git a/digital/io/src/defs.h b/digital/io/src/defs.h index 507e2766..d80ff96d 100644 --- a/digital/io/src/defs.h +++ b/digital/io/src/defs.h @@ -40,4 +40,7 @@ struct position_t }; typedef struct position_t position_t; +/** Convert degrees to an angle usable in position_t. */ +#define POSITION_A_DEG(a) G_ANGLE_UF016_DEG (a) + #endif /* defs_h */ diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h index 0ff7a11c..1ec6e47e 100644 --- a/digital/io/src/playground.h +++ b/digital/io/src/playground.h @@ -34,6 +34,7 @@ * see it with the two start zone at the top of the scheme). */ +#include "defs.h" #include "bot.h" /** @@ -55,15 +56,25 @@ * Considering there is a symmetry axis on X, this macro will compute the * value for on the X axis depending on the color. */ -#define PG_X_VALUE_COMPUTING(x) \ - (bot_color ? (x) : PG_WIDTH - (x)) +#define PG_X(x) (bot_color ? (x) : PG_WIDTH - (x)) + +/** Same as PG_Y, but for Y coordinate. Actually nothing is done, there is no + * symmetry. */ +#define PG_Y(y) (y) /** * Considering there is a symmetry axis on X, this macro will compute the * value of the angle depending on the color. + * + * Takes degrees as input. */ -#define PG_A_VALUE_COMPUTING(a) \ - (bot_color ? (a) : (BOT_ANGLE_DEGREE * 180) - (a)) +#define PG_A_DEG(a) \ + (bot_color ? POSITION_A_DEG (a) : POSITION_A_DEG (180 - (a))) + +/** Initialiser for position_t applying symmetry according to color. Takes + * degrees for angle. */ +#define PG_POSITION_DEG(x, y, a) \ + { { PG_X (x), PG_Y (y) }, PG_A_DEG (a) } /** * Start zone. -- cgit v1.2.3