From fa8ff2dec76b1b366207a126dfc360331510c50d Mon Sep 17 00:00:00 2001 From: schodet Date: Fri, 19 Nov 2004 16:38:34 +0000 Subject: Ajout du calcul de position. Non testé. --- n/asserv/src/avrconfig.h | 2 +- n/asserv/src/main.c | 30 ++++++++++++++-- n/asserv/src/postrack.c | 94 ++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 123 insertions(+), 3 deletions(-) create mode 100644 n/asserv/src/postrack.c (limited to 'n/asserv') diff --git a/n/asserv/src/avrconfig.h b/n/asserv/src/avrconfig.h index 16d55a7..bb19576 100644 --- a/n/asserv/src/avrconfig.h +++ b/n/asserv/src/avrconfig.h @@ -47,7 +47,7 @@ /** Stop bits : 1, 2. */ #define AC_RS232_STOP_BITS 1 /** Send buffer size, should be power of 2 for RING mode. */ -#define AC_RS232_SEND_BUFFER_SIZE 32 +#define AC_RS232_SEND_BUFFER_SIZE 64 /** Recv buffer size, should be power of 2 for RING mode. */ #define AC_RS232_RECV_BUFFER_SIZE 32 /** Select serial port (0 or 1). */ diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c index 8656644..2adbb76 100644 --- a/n/asserv/src/main.c +++ b/n/asserv/src/main.c @@ -33,6 +33,7 @@ #include "timer.c" #include "counter.c" #include "speed.c" +#include "postrack.c" /** Motor mode : * 0 - pwm setup; @@ -55,9 +56,12 @@ uint8_t motor_stat_timer, motor_stat_timer_cpt; /** Report of counters. */ uint8_t motor_stat_counter, motor_stat_counter_cpt; +/** Report position. */ +uint8_t motor_stat_postrack, motor_stat_postrack_cpt; + /** Record timer value at different stage of computing. Used for performance * analisys. */ -uint8_t motor_timer_0, motor_timer_1, motor_timer_2; +uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3; /* +AutoDec */ @@ -80,6 +84,7 @@ main (void) timer_init (); counter_init (); speed_init (); + postrack_init (); rs232_init (); proto_init (proto_callback, rs232_putc); proto_send0 ('z'); @@ -98,6 +103,8 @@ main_loop (void) while (!timer_pending ()) counter_poll (); counter_update (); + motor_timer_3 = timer_read (); + postrack_update (); motor_timer_2 = timer_read (); /* Speed control. */ if (motor_mode >= 1) @@ -129,7 +136,8 @@ main_loop (void) } if (motor_stat_timer && !--motor_stat_timer_cpt) { - proto_send3 ('T', motor_timer_2, motor_timer_1, motor_timer_0); + proto_send4 ('T', motor_timer_3, motor_timer_2, motor_timer_1, + motor_timer_0); motor_stat_timer_cpt = motor_stat_timer; } if (motor_stat_counter && !--motor_stat_counter_cpt) @@ -138,6 +146,18 @@ main_loop (void) counter_left >> 8, counter_left, counter_right >> 8, counter_right); motor_stat_counter_cpt = motor_stat_counter; + if (motor_stat_postrack && !--motor_stat_postrack_cpt) + { + proto_send4 ('X', + postrack_x >> 24, postrack_x >> 16, + postrack_x >> 8, postrack_x); + proto_send4 ('Y', + postrack_y >> 24, postrack_y >> 16, + postrack_y >> 8, postrack_y); + proto_send4 ('A', + postrack_a >> 24, postrack_a >> 16, + postrack_a >> 8, postrack_a); + motor_stat_postrack_cpt = motor_stat_postrack; } /* Misc. */ if ((motor_loop_cpt & 7) == 0) @@ -185,6 +205,9 @@ proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]) case c ('i', 2): speed_ki = argv[0] << 8 | argv[1]; break; + case c ('f', 2): + postrack_set_footing (argv[0] << 8 | argv[1]); + break; case c ('m', 1): motor_stat_speed_cpt = motor_stat_speed = argv[0]; motor_stat_pwm_cpt = motor_stat_pwm = argv[0]; @@ -195,6 +218,9 @@ proto_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[]) case c ('t', 1): motor_stat_timer_cpt = motor_stat_timer = argv[0]; break; + case c ('T', 1): + motor_stat_postrack_cpt = motor_stat_postrack = argv[0]; + break; default: proto_send0 ('e'); return; diff --git a/n/asserv/src/postrack.c b/n/asserv/src/postrack.c new file mode 100644 index 0000000..0b6d196 --- /dev/null +++ b/n/asserv/src/postrack.c @@ -0,0 +1,94 @@ +/* postrack.c */ +/* asserv - Position & speed motor control on a ATmega128. {{{ + * + * Copyright (C) 2004 Nicolas Schodet + * + * Robot APB Team/Efrei 2005. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * 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. + * + * }}} */ + +/** Current position, f24.8. */ +int32_t postrack_x, postrack_y; +/** Current angle, f8.24. */ +int32_t postrack_a; +/** Distance between the weels, u16. */ +uint16_t postrack_footing; +/** Precomputed footing factor, f8.24. + * postrack_footing_factor = 1 / postrack_footing */ +int32_t postrack_footing_factor; + +/* +AutoDec */ + +/** Initialise the position tracker. */ +static inline void +postrack_init (void); + +/** Update the current position. */ +static inline void +postrack_update (void); + +/** Change the footing value. */ +static inline void +postrack_set_footing (uint16_t footing); + +/* -AutoDec */ + +/** Initialise the position tracker. */ +static inline void +postrack_init (void) +{ + postrack_set_footing (5000); +} + +/** Update the current position. */ +static inline void +postrack_update (void) +{ + int32_t d, dd, da, r, na; + d = counter_right_diff + counter_left_diff; + d <<= 24 - 1; + if (counter_right_diff == counter_left_diff) + { + postrack_x += dsp_mul_f824 (d, dsp_cos (postrack_a)) >> 16; + postrack_y += dsp_mul_f824 (d, dsp_sin (postrack_a)) >> 16; + } + else + { + /* XXX: WARNING: dleft & dright 16 bits? + * This code should not work at high speed (> 127). */ + dd = counter_right_diff - counter_left_diff; + dd <<= 24; + da = dsp_mul_f824 (postrack_footing_factor, dd); + r = dsp_div_f824 (d, da); + na = postrack_a + da; + postrack_x += + dsp_mul_f824 (r, dsp_sin (na) - dsp_sin (postrack_a)) >> 16; + postrack_y += + dsp_mul_f824 (r, dsp_cos (postrack_a) - dsp_cos (na)) >> 16; + postrack_a = na; + } +} + +/** Change the footing value. */ +static inline void +postrack_set_footing (uint16_t footing) +{ + postrack_footing = footing; + postrack_footing_factor = (1L << 24) / footing; +} -- cgit v1.2.3