From e7d3f72a1b83a67cd9ac5b366de6fb2be0437166 Mon Sep 17 00:00:00 2001 From: schodet Date: Sat, 1 Apr 2006 17:58:15 +0000 Subject: Ajout du calcul de position absolu. --- n/asserv/src/asserv/eeprom.avr.c | 6 ++ n/asserv/src/asserv/main.c | 47 ++++++++++++++++ n/asserv/src/asserv/postrack.c | 115 +++++++++++++++++++++++++++++++++++++++ 3 files changed, 168 insertions(+) create mode 100644 n/asserv/src/asserv/postrack.c (limited to 'n/asserv/src/asserv') diff --git a/n/asserv/src/asserv/eeprom.avr.c b/n/asserv/src/asserv/eeprom.avr.c index 34770bb..cefea42 100644 --- a/n/asserv/src/asserv/eeprom.avr.c +++ b/n/asserv/src/asserv/eeprom.avr.c @@ -39,6 +39,9 @@ eeprom_read_params (void) return; pwm_dir = eeprom_read_byte (p8++); p16 = (uint16_t *) p8; + postrack_set_footing (eeprom_read_word (p16++)); + speed_theta_acc = eeprom_read_word (p16++); + speed_alpha_acc = eeprom_read_word (p16++); pos_theta_kp = eeprom_read_word (p16++); pos_alpha_kp = eeprom_read_word (p16++); pos_theta_ki = eeprom_read_word (p16++); @@ -58,6 +61,9 @@ eeprom_write_params (void) eeprom_write_byte (p8++, EEPROM_KEY); eeprom_write_byte (p8++, pwm_dir); p16 = (uint16_t *) p8; + eeprom_write_word (p16++, postrack_footing); + eeprom_write_word (p16++, speed_theta_acc); + eeprom_write_word (p16++, speed_alpha_acc); eeprom_write_word (p16++, pos_theta_kp); eeprom_write_word (p16++, pos_alpha_kp); eeprom_write_word (p16++, pos_theta_ki); diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c index 11ef09a..c74f786 100644 --- a/n/asserv/src/asserv/main.c +++ b/n/asserv/src/asserv/main.c @@ -42,6 +42,7 @@ #endif #include "pos.c" #include "speed.c" +#include "postrack.c" #ifndef HOST # include "eeprom.avr.c" #endif @@ -56,6 +57,9 @@ int8_t main_mode; /** Report of counters. */ uint8_t main_stat_counter, main_stat_counter_cpt; +/** Report of position. */ +uint8_t main_stat_postrack, main_stat_postrack_cpt; + /** Statistics about speed control. */ uint8_t main_stat_speed, main_stat_speed_cpt; @@ -121,6 +125,8 @@ main_loop (void) /* Pwm setup. */ pwm_update (); main_timer_2 = timer_read (); + /* Compute absolute position. */ + postrack_update (); /* Speed control. */ if (main_mode >= 2) speed_update (); @@ -131,6 +137,11 @@ main_loop (void) proto_send2w ('C', counter_left, counter_right); main_stat_counter_cpt = main_stat_counter; } + if (main_stat_postrack && !--main_stat_postrack_cpt) + { + proto_send3d ('X', postrack_x, postrack_y, postrack_a); + main_stat_postrack_cpt = main_stat_postrack; + } if (main_stat_speed && !--main_stat_speed_cpt) { proto_send2b ('S', speed_theta_cur >> 8, speed_alpha_cur >> 8); @@ -172,6 +183,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) switch (c (cmd, size)) { case c ('z', 0): + /* Reset. */ utils_reset (); break; /* Commands. */ @@ -240,6 +252,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Counter stats. */ main_stat_counter_cpt = main_stat_counter = args[0]; break; + case c ('X', 1): + /* Position stats. */ + main_stat_postrack_cpt = main_stat_postrack = args[0]; + break; case c ('S', 1): /* Motor speed control stats. */ main_stat_speed_cpt = main_stat_speed = args[0]; @@ -266,7 +282,36 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) { switch (c (args[0], size)) { + case c ('X', 1): + /* Reset position. */ + postrack_x = 0; + postrack_y = 0; + postrack_a = 0; + break; + case c ('X', 5): + /* Set current x position. + * - d: x position. */ + postrack_x = v8_to_v32 (args[1], args[2], args[3], args[4]); + break; + case c ('Y', 5): + /* Set current y position. + * - d: y position. */ + postrack_y = v8_to_v32 (args[1], args[2], args[3], args[4]); + break; + case c ('A', 5): + /* Set current angle. + * - d: angle. */ + postrack_a = v8_to_v32 (args[1], args[2], args[3], args[4]); + break; + case c ('f', 3): + /* Set footing. + * - w: footing. */ + postrack_set_footing (v8_to_v16 (args[1], args[2])); + break; case c ('a', 5): + /* Set acceleration. + * - w: theta. + * - w: alpha. */ speed_theta_acc = v8_to_v16 (args[1], args[2]); speed_alpha_acc = v8_to_v16 (args[3], args[4]); break; @@ -316,6 +361,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('P', 1): /* Print current settings. */ proto_send1b ('E', EEPROM_KEY); + proto_send1w ('f', postrack_footing); + proto_send2w ('a', speed_theta_acc, speed_alpha_acc); proto_send2w ('p', pos_theta_kp, pos_alpha_kp); proto_send2w ('i', pos_theta_ki, pos_alpha_ki); proto_send2w ('d', pos_theta_kd, pos_alpha_kd); diff --git a/n/asserv/src/asserv/postrack.c b/n/asserv/src/asserv/postrack.c new file mode 100644 index 0000000..ae13ec9 --- /dev/null +++ b/n/asserv/src/asserv/postrack.c @@ -0,0 +1,115 @@ +/* postrack.c */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2006 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/2pi * 256) / postrack_footing + * Explanations: + * - Angles are between 0 and 1, corresponding to 0 and 2pi, therefore we + * must divide by 2pi to convert unit (Arc=Angle * Radius only works for + * radians). + * - dd (see postrack_update) is in f11.16 format, we multiply by 256 to have + * a angle in f8.24 format. + * - this factor is in f8.24 format, therefore, 1 is writen (1L << 24). */ +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) +{ + /* Prevent division by 0 by providing a default large value. */ + postrack_set_footing (0x1000); +} + +#define M_PI 3.14159265358979323846 /* pi */ + +/** Update the current position. */ +static inline void +postrack_update (void) +{ + int32_t d, dd, da, na, dsc; + d = counter_right_diff + counter_left_diff; /* 10b */ + d <<= 16; /* 10.16b */ + if (counter_right_diff == counter_left_diff) + { + /* Line. */ + postrack_x += fixed_mul_f824 (d, fixed_cos_f824 (postrack_a)) >> 8; + postrack_y += fixed_mul_f824 (d, fixed_sin_f824 (postrack_a)) >> 8; + } + else + { + /* Arc. */ + dd = counter_right_diff - counter_left_diff; /* 10b */ + dd <<= 16; /* 10.16b */ + da = fixed_mul_f824 (dd, postrack_footing_factor);/* 8.24b */ + /* New angle. */ + na = postrack_a + da; + /* Compute da in radians. */ + da = fixed_mul_f824 (da, 2 * M_PI * (1L << 24)); + /* X increment. */ + dsc = fixed_sin_f824 (na) - fixed_sin_f824 (postrack_a); /* 8.24b */ + dsc = fixed_div_f824 (dsc, da); /* 8.24b < 1 */ + postrack_x += fixed_mul_f824 (d, dsc) >> 8; /* 24.8b */ + /* Y increment. */ + dsc = fixed_cos_f824 (postrack_a) - fixed_cos_f824 (na); /* 8.24b */ + dsc = fixed_div_f824 (dsc, da); /* 8.24b < 1 */ + postrack_y += fixed_mul_f824 (d, dsc) >> 8; /* 24.8b */ + /* Angle update. */ + postrack_a = na; + postrack_a &= 0x00ffffff; + } +} + +#define M_1_PI 0.31830988618379067154 /* 1/pi */ + +/** Change the footing value. */ +static inline void +postrack_set_footing (uint16_t footing) +{ + postrack_footing = footing; + postrack_footing_factor = + (int32_t) (0.5 * M_1_PI * (1L << 8) * (1L << 24)) / footing; +} -- cgit v1.2.3