From 1c5d5350411f25fde6168c301b68fe7fe125cf0e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 13 Apr 2010 00:21:38 +0200 Subject: digital/io/src: add new position_t type --- digital/io/src/ai_move_cb.c | 24 +++++++++++------------- digital/io/src/ai_top_cb.c | 18 +++++++++--------- digital/io/src/asserv.c | 23 ++++++++++++++++++----- digital/io/src/asserv.h | 16 ++-------------- digital/io/src/defs.h | 43 +++++++++++++++++++++++++++++++++++++++++++ digital/io/src/main.c | 13 +++++-------- digital/io/src/move.c | 9 ++------- digital/io/src/move.h | 20 +++----------------- 8 files changed, 93 insertions(+), 73 deletions(-) create mode 100644 digital/io/src/defs.h diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c index eb0da148..bfa033f6 100644 --- a/digital/io/src/ai_move_cb.c +++ b/digital/io/src/ai_move_cb.c @@ -92,11 +92,11 @@ move_get_next_position (void) if (move_data.final_move) return 2; /* Get the current position */ - asserv_position_t current_pos; + position_t current_pos; asserv_get_position (¤t_pos); /* Give the current position of the bot to the path module */ - path_endpoints (current_pos.x, current_pos.y, - move_data.final.x, move_data.final.y); + path_endpoints (current_pos.v.x, current_pos.v.y, + move_data.final.v.x, move_data.final.v.y); /* Update the path module */ path_update (); @@ -104,7 +104,7 @@ move_get_next_position (void) if (path_get_next (&dst.x, &dst.y) != 0) { /* If it is not the last position. */ - if (dst.x != move_data.final.x || dst.y != move_data.final.y) + if (dst.x != move_data.final.v.x || dst.y != move_data.final.v.y) { /* Not final position. */ move_data.final_move = 0; @@ -119,8 +119,9 @@ move_get_next_position (void) asserv_goto_xya (dst.x, dst.y, move_data.final.a, move_data.backward_movement_allowed); } - TRACE (TRACE_MOVE__GO_TO, (u16) current_pos.x, (u16) current_pos.y, - current_pos.a, dst.x, dst.y, move_data.final.a); + TRACE (TRACE_MOVE__GO_TO, (u16) current_pos.v.x, + (u16) current_pos.v.y, current_pos.a, dst.x, dst.y, + move_data.final.a); /* Reset try counter. */ move_data.try_again_counter = 3; return 1; @@ -139,8 +140,7 @@ move_get_next_position (void) * @param obstacle the obstacle position computed */ void -move_compute_obstacle_position (asserv_position_t cur, - vect_t *obstacle) +move_compute_obstacle_position (position_t cur, vect_t *obstacle) { int16_t dist; @@ -157,11 +157,9 @@ move_compute_obstacle_position (asserv_position_t cur, } /* X */ - obstacle->x = cur.x + fixed_mul_f824 (fixed_cos_f824 (angle), - dist); + obstacle->x = cur.v.x + fixed_mul_f824 (fixed_cos_f824 (angle), dist); /* Y */ - obstacle->y = cur.y + fixed_mul_f824 (fixed_sin_f824 (angle), - dist); + obstacle->y = cur.v.y + fixed_mul_f824 (fixed_sin_f824 (angle), dist); } /** @@ -172,7 +170,7 @@ move_obstacle_here (void) { vect_t obstacle; /* Get the current position of the bot */ - asserv_position_t current; + position_t current; asserv_get_position (¤t); /* Compute the obstacle position */ move_compute_obstacle_position (current, &obstacle); diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c index 9c47e5ca..91937ad2 100644 --- a/digital/io/src/ai_top_cb.c +++ b/digital/io/src/ai_top_cb.c @@ -58,9 +58,9 @@ ai__IDLE__start (void) fsm_branch_t ai__WAIT_INIT_TO_FINISH__init_match_is_started (void) { - asserv_position_t pos; - pos.x = PG_X_VALUE_COMPUTING (2000); - pos.y = 1000; + position_t pos; + pos.v.x = PG_X_VALUE_COMPUTING (2000); + pos.v.y = 1000; pos.a = PG_A_VALUE_COMPUTING (0); move_start (pos, 0); return ai_next (WAIT_INIT_TO_FINISH, init_match_is_started); @@ -74,9 +74,9 @@ ai__WAIT_INIT_TO_FINISH__init_match_is_started (void) fsm_branch_t ai__GO_FAR__move_fsm_succeed (void) { - asserv_position_t pos; - pos.x = PG_X_VALUE_COMPUTING (1000); - pos.y = 1000; + position_t pos; + pos.v.x = PG_X_VALUE_COMPUTING (1000); + pos.v.y = 1000; pos.a = PG_A_VALUE_COMPUTING (0); move_start (pos, 0); return ai_next (GO_FAR, move_fsm_succeed); @@ -90,9 +90,9 @@ ai__GO_FAR__move_fsm_succeed (void) fsm_branch_t ai__GO_NEAR__move_fsm_succeed (void) { - asserv_position_t pos; - pos.x = PG_X_VALUE_COMPUTING (2000); - pos.y = 1000; + position_t pos; + pos.v.x = PG_X_VALUE_COMPUTING (2000); + pos.v.y = 1000; pos.a = PG_A_VALUE_COMPUTING (0); move_start (pos, 0); return ai_next (GO_NEAR, move_fsm_succeed); diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index 0ae1465f..87c3baa4 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -93,6 +93,19 @@ static uint8_t asserv_twi_buffer[15]; */ static uint8_t *asserv_twi_buffer_param = &asserv_twi_buffer[3]; +/** + * Structure for storing a position for the bot using asserv units. + */ +typedef struct asserv_position_t +{ + /** X position. */ + uint32_t x; + /** Y position. */ + uint32_t y; + /** Angle. */ + uint16_t a; +} asserv_position_t; + /** * Status structure maintains by the update command. */ @@ -359,15 +372,15 @@ asserv_motor1_cmd_status (void) /* Get the current position of the bot. */ void -asserv_get_position (asserv_position_t *current_position) +asserv_get_position (position_t *current_position) { if (current_position) { /* Copy last received status buffer information to current position */ - current_position->x = fixed_mul_f824 (asserv_status.position.x, - asserv_scale); - current_position->y = fixed_mul_f824 (asserv_status.position.y, - asserv_scale); + current_position->v.x = fixed_mul_f824 (asserv_status.position.x, + asserv_scale); + current_position->v.y = fixed_mul_f824 (asserv_status.position.y, + asserv_scale); current_position->a = asserv_status.position.a; } } diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h index 7e94d19b..269c138d 100644 --- a/digital/io/src/asserv.h +++ b/digital/io/src/asserv.h @@ -24,6 +24,7 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "defs.h" /** * @file Control the asserv board from io using the TWI protocol. @@ -132,25 +133,12 @@ asserv_motor0_cmd_status (void); asserv_status_e asserv_motor1_cmd_status (void); -/** - * Structure for storing a position for the bot. - */ -typedef struct asserv_position_t -{ - /** X position. */ - uint32_t x; - /** Y position. */ - uint32_t y; - /** Angle. */ - uint16_t a; -} asserv_position_t; - /** * Get the current position of the bot. * @param current_position the current position to update. */ void -asserv_get_position (asserv_position_t *current_position); +asserv_get_position (position_t *current_position); /** * Get the motor0 position. diff --git a/digital/io/src/defs.h b/digital/io/src/defs.h new file mode 100644 index 00000000..507e2766 --- /dev/null +++ b/digital/io/src/defs.h @@ -0,0 +1,43 @@ +#ifndef defs_h +#define defs_h +/* defs.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "modules/math/geometry/geometry.h" +#include "modules/math/geometry/vect.h" + +/** General purpose defines. */ + +/** Structure defining a position with an angle. */ +struct position_t +{ + /** Cartesian position in millimeters, (0, 0) is at bottom left. */ + vect_t v; + /** Angle, counter-clockwise, [0, 1), f0.16. + * For example, 0x8000 means 0.5, which means 180 degrees. */ + uint16_t a; +}; +typedef struct position_t position_t; + +#endif /* defs_h */ diff --git a/digital/io/src/main.c b/digital/io/src/main.c index bb0a0205..fdc4bfcc 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -250,12 +250,9 @@ main_loop (void) { uint8_t obs_nb; vect_t obs_pos[2]; - asserv_position_t cur_pos; - vect_t robot_pos; - robot_pos.x = cur_pos.x; - robot_pos.y = cur_pos.y; - asserv_get_position (&cur_pos); - obs_nb = radar_update (robot_pos, cur_pos.a, obs_pos); + position_t robot_pos; + asserv_get_position (&robot_pos); + obs_nb = radar_update (robot_pos.v, robot_pos.a, obs_pos); simu_send_pos_report (obs_pos, obs_nb, 0); } #endif @@ -291,10 +288,10 @@ main_loop (void) if (main_stats_asserv_ && !--main_stats_asserv_cpt_) { /* Get current position */ - asserv_position_t cur_pos; + position_t cur_pos; asserv_get_position (&cur_pos); /* Send stats */ - proto_send3w ('A', cur_pos.x, cur_pos.y, cur_pos.a); + proto_send3w ('A', cur_pos.v.x, cur_pos.v.y, cur_pos.a); /* Reset stats counter */ main_stats_asserv_cpt_ = main_stats_asserv_; } diff --git a/digital/io/src/move.c b/digital/io/src/move.c index 4343e60f..69f71b31 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -23,24 +23,19 @@ * * }}} */ #include "common.h" -#include "asserv.h" #include "move.h" #include "fsm.h" -#include "main.h" - /** * Internal data used by the move FSM. */ struct move_data_t move_data; void -move_start (asserv_position_t position, uint8_t backward) +move_start (position_t position, uint8_t backward) { /* Set parameters. */ - move_data.final.x = position.x; - move_data.final.y = position.y; - move_data.final.a = position.a; + move_data.final= position; 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 8eb485a0..8ea9eed4 100644 --- a/digital/io/src/move.h +++ b/digital/io/src/move.h @@ -25,21 +25,7 @@ * * }}} */ -#include "modules/math/geometry/vect.h" -#include "asserv.h" - -/** - * A position. - */ -typedef struct move_position_t -{ - /** X position. */ - int16_t x; - /** Y position. */ - int16_t y; - /** A angle. */ - uint16_t a; -} move_position_t; +#include "defs.h" /** * Move FSM associated data. @@ -47,7 +33,7 @@ typedef struct move_position_t struct move_data_t { /** Final position. */ - move_position_t final; + position_t final; /** Backward direction allowed flag. */ uint8_t backward_movement_allowed; /** Try again counter. */ @@ -68,6 +54,6 @@ extern struct move_data_t move_data; * backward is compulsary. */ void -move_start (asserv_position_t position, uint8_t backward); +move_start (position_t position, uint8_t backward); #endif /* move_h */ -- cgit v1.2.3