From bc38b408c53281ee985f6f9d67accc5733d99e37 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 28 Jan 2008 21:12:12 +0100 Subject: * digital/asserv/src/asserv: - imported old sources. - removed holes support (rule 2006). --- digital/asserv/src/asserv/postrack.c | 116 +++++++++++++++++++++++++++++++++++ 1 file changed, 116 insertions(+) create mode 100644 digital/asserv/src/asserv/postrack.c (limited to 'digital/asserv/src/asserv/postrack.c') diff --git a/digital/asserv/src/asserv/postrack.c b/digital/asserv/src/asserv/postrack.c new file mode 100644 index 00000000..6da77592 --- /dev/null +++ b/digital/asserv/src/asserv/postrack.c @@ -0,0 +1,116 @@ +/* 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 = (2 * 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 f10.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). + * - there is a two factor because of the sum done in the motor control. */ +uint32_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 = + (uint32_t) (M_1_PI * (1L << 8) * (1L << 24)) / footing; +} -- cgit v1.2.3