summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io/src')
-rw-r--r--digital/io/src/Makefile2
-rw-r--r--digital/io/src/asserv.c33
-rw-r--r--digital/io/src/asserv.h8
-rw-r--r--digital/io/src/giboulee.h9
4 files changed, 48 insertions, 4 deletions
diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile
index 7b463940..e170c70c 100644
--- a/digital/io/src/Makefile
+++ b/digital/io/src/Makefile
@@ -8,7 +8,7 @@ io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c trap.c sharp.c \
gutter_fsm.c gutter_cb.c gutter.c \
move.c move_fsm.c move_cb.c \
top.c top_fsm.c top_cb.c
-MODULES = proto uart twi utils adc
+MODULES = proto uart twi utils adc math/fixed
CONFIGFILE = avrconfig.h
# atmega8, atmega8535, atmega128...
AVR_MCU = atmega128
diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c
index 9addd320..14a745ca 100644
--- a/digital/io/src/asserv.c
+++ b/digital/io/src/asserv.c
@@ -28,15 +28,22 @@
#include "modules/twi/twi.h" /* twi_* */
#include "modules/utils/byte.h" /* v*_to_v* */
+#include "modules/math/fixed/fixed.h"
#include "giboulee.h" /* BOT_* */
#include "io.h"
+/** Scaling factor. */
+uint32_t asserv_scale;
+
/**
* @defgroup AsservPrivate Asserv module private variables and functions
* declarations and definitions
* @{
*/
+/** Scaling factor inverse. */
+static uint32_t asserv_scale_inv;
+
/**
* Sequence number.
* It is used for the acknowledge of the command sent to the asserv.
@@ -127,6 +134,7 @@ asserv_twi_send_command (uint8_t command, uint8_t length);
*/
static inline uint8_t
asserv_twi_send (uint8_t length);
+
/**
* Move the arm.
* A complete rotation correspond to 5000 steps.
@@ -210,6 +218,8 @@ asserv_init (void)
twi_init (AC_IO_TWI_ADDRESS);
/* We are at first command */
asserv_twi_seq = asserv_status.seq = 0;
+ /* Scaling factor. */
+ asserv_set_scale (BOT_SCALE * (1L << 24));
}
/* Update the status of the asserv board seen from the io program. */
@@ -294,8 +304,10 @@ asserv_get_position (asserv_position_t *current_position)
if (current_position)
{
/* Copy last received status buffer information to current position */
- current_position->x = asserv_status.position.x;
- current_position->y = asserv_status.position.y;
+ 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->a = asserv_status.position.a;
}
}
@@ -336,6 +348,7 @@ asserv_stop_motor (void)
void
asserv_move_linearly (int32_t distance)
{
+ distance = fixed_mul_f824 (distance, asserv_scale_inv);
/* Put distance as parameter */
asserv_twi_buffer_param[0] = v32_to_v8 (distance, 2);
asserv_twi_buffer_param[1] = v32_to_v8 (distance, 1);
@@ -370,6 +383,8 @@ asserv_goto_angle (int16_t angle)
void
asserv_goto_xya (uint32_t x, uint32_t y, int16_t a)
{
+ x = fixed_mul_f824 (x, asserv_scale_inv);
+ y = fixed_mul_f824 (y, asserv_scale_inv);
/* Put X as parameter */
asserv_twi_buffer_param[0] = v32_to_v8 (x, 2);
asserv_twi_buffer_param[1] = v32_to_v8 (x, 1);
@@ -428,6 +443,7 @@ asserv_move_arm (int16_t offset, uint8_t speed)
void
asserv_set_x_position (int32_t x)
{
+ x = fixed_mul_f824 (x, asserv_scale_inv);
/* 'X' subcommand */
asserv_twi_buffer_param[0] = 'X';
/* Put x position as parameter */
@@ -443,6 +459,7 @@ asserv_set_x_position (int32_t x)
void
asserv_set_y_position (int32_t y)
{
+ y = fixed_mul_f824 (y, asserv_scale_inv);
/* 'Y' subcommand */
asserv_twi_buffer_param[0] = 'Y';
/* Put y position as parameter */
@@ -489,6 +506,8 @@ asserv_set_speed (uint8_t linear_high, uint8_t angular_high,
void
asserv_set_position (int32_t x, int32_t y, int16_t angle)
{
+ x = fixed_mul_f824 (x, asserv_scale_inv);
+ y = fixed_mul_f824 (y, asserv_scale_inv);
/* 'X' subcommand */
asserv_twi_buffer_param[0] = 'X';
/* Put x position as parameter */
@@ -515,6 +534,8 @@ asserv_set_position (int32_t x, int32_t y, int16_t angle)
void
asserv_goto (uint32_t x, uint32_t y)
{
+ x = fixed_mul_f824 (x, asserv_scale_inv);
+ y = fixed_mul_f824 (y, asserv_scale_inv);
/* Put X as parameter */
asserv_twi_buffer_param[0] = v32_to_v8 (x, 2);
asserv_twi_buffer_param[1] = v32_to_v8 (x, 1);
@@ -549,3 +570,11 @@ asserv_arm_position_reached (void)
return 0;
}
+/* Set scale. */
+void
+asserv_set_scale (uint32_t scale)
+{
+ asserv_scale = scale;
+ asserv_scale_inv = fixed_div_f824 (1L << 24, scale);
+}
+
diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h
index 56915efa..686cddca 100644
--- a/digital/io/src/asserv.h
+++ b/digital/io/src/asserv.h
@@ -163,7 +163,7 @@ asserv_stop_motor (void);
/**
* Move linearly.
* Move class command.
- * @param distance the distance to move (in step).
+ * @param distance the distance to move (mm).
*/
void
asserv_move_linearly (int32_t distance);
@@ -295,4 +295,10 @@ asserv_arm_set_position_reached (uint16_t position);
uint8_t
asserv_arm_position_reached (void);
+/** Set scale.
+ * @param scale vous avez qu'à deviner (f8.24).
+ */
+void
+asserv_set_scale (uint32_t scale);
+
#endif /* asserv_h */
diff --git a/digital/io/src/giboulee.h b/digital/io/src/giboulee.h
index 0dae6098..2e3706c8 100644
--- a/digital/io/src/giboulee.h
+++ b/digital/io/src/giboulee.h
@@ -67,6 +67,15 @@
#define BOT_ANGLE_DEGREE (65536 / 360)
/**
+ * The scaling factor, millimeter per step.
+ */
+#ifdef HOST
+# define BOT_SCALE 0.0395840674352314
+#else
+# define BOT_SCALE 0.0413359788359788
+#endif
+
+/**
* Definition of the colors.
*/
enum team_color_e