From 7b3882a2319031acc968c00021379d37091e7f62 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 23 Apr 2008 09:19:23 +0200 Subject: * digital/io/src: - added scaling factor. --- digital/io/src/Makefile | 2 +- digital/io/src/asserv.c | 33 +++++++++++++++++++++++++++++++-- digital/io/src/asserv.h | 8 +++++++- digital/io/src/giboulee.h | 9 +++++++++ 4 files changed, 48 insertions(+), 4 deletions(-) (limited to 'digital/io') 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 @@ -66,6 +66,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. */ -- cgit v1.2.3