summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2010-04-13 00:21:38 +0200
committerNicolas Schodet2010-04-13 00:21:38 +0200
commit1c5d5350411f25fde6168c301b68fe7fe125cf0e (patch)
tree346ba58167efb828dfed6f8773c625b54218199d /digital
parentafa6ecacdd5183b9e751184987f2929c811e4beb (diff)
digital/io/src: add new position_t type
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/ai_move_cb.c24
-rw-r--r--digital/io/src/ai_top_cb.c18
-rw-r--r--digital/io/src/asserv.c23
-rw-r--r--digital/io/src/asserv.h16
-rw-r--r--digital/io/src/defs.h43
-rw-r--r--digital/io/src/main.c13
-rw-r--r--digital/io/src/move.c9
-rw-r--r--digital/io/src/move.h20
8 files changed, 93 insertions, 73 deletions
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 (&current_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 (&current);
/* 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
@@ -94,6 +94,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.
*/
typedef struct asserv_struct_s
@@ -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.
@@ -133,24 +134,11 @@ 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 */