From e4589d3099a11723b3cd017c9c8c97b070455ea9 Mon Sep 17 00:00:00 2001 From: schodet Date: Fri, 11 Feb 2005 14:36:33 +0000 Subject: Ajout de la première version d'asservissement en position. --- n/asserv/src/dsp.c | 23 +++++++++++++++++++++++ n/asserv/src/goto.c | 50 ++++++++++++++++++++++++++++++++++++++++++++++++++ n/asserv/src/main.c | 41 ++++++++++++++++++++++++++++++++++------- n/asserv/src/speed.c | 36 ++++++++++++++++++++++++++++++++++++ 4 files changed, 143 insertions(+), 7 deletions(-) create mode 100644 n/asserv/src/goto.c (limited to 'n') diff --git a/n/asserv/src/dsp.c b/n/asserv/src/dsp.c index 4484b2f..5bbcf65 100644 --- a/n/asserv/src/dsp.c +++ b/n/asserv/src/dsp.c @@ -345,3 +345,26 @@ dsp_sin (int32_t a) return -dsp_cos_dli (a - (1L << 23) - (1L << 22)); } +/** Compute square root, uf24.8. */ +uint32_t +dsp_sqrt (uint32_t x) +{ + uint32_t root, bit, test; + root = 0; + bit = 1L << 30; + do + { + test = root + bit; + //printf ("test = 0x%x, root = 0x%x, bit = 0x%x\n", test, root, bit); + if (x >= test) + { + x -= test; + root = test + bit; + //printf ("x = 0x%x, root = 0x%x\n", x, root); + } + root >>= 1; + bit >>= 2; + } while (bit); + return root << 4; +} + diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c new file mode 100644 index 0000000..75948a8 --- /dev/null +++ b/n/asserv/src/goto.c @@ -0,0 +1,50 @@ +/* goto.c */ +/* asserv - Position & speed motor control on a ATmega128. {{{ + * + * Copyright (C) 2005 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. + * + * }}} */ + +/** Destination position, f24.8. */ +int32_t goto_x, goto_y; +/** Destination angle, f8.24. */ +int32_t goto_a; + +/* +AutoDec */ +/* -AutoDec */ + +/** Update the speed according to the desired destination. */ +static inline void +goto_update (void) +{ + int32_t dx, dy, c, s, dl, da; + /* Project in the robot base. */ + dx = goto_x - postrack_x; /* f24.8 */ + dy = goto_y - postrack_y; + c = dsp_cos (postrack_a); + s = dsp_sin (postrack_a); + dl = dsp_mul_f824 (dx, c) + dsp_mul_f824 (dy, s); + da = dsp_mul_f824 (dy, c) - dsp_mul_f824 (dx, s); + /* Convert da into a arc. This is a rough aproximation. */ + da = da * postrack_footing / 2 / (dl >> 8); + speed_distance (dl, da); +} + diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c index 3fed431..bac4cfd 100644 --- a/n/asserv/src/main.c +++ b/n/asserv/src/main.c @@ -34,6 +34,7 @@ #include "counter.c" #include "speed.c" #include "postrack.c" +#include "goto.c" /** Motor mode : * 0 - pwm setup; @@ -61,7 +62,8 @@ 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, motor_timer_3; +uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3, + motor_timer_4; /* +AutoDec */ @@ -98,8 +100,15 @@ main_loop (void) while (!timer_pending ()) counter_poll (); counter_update (); - motor_timer_3 = timer_read (); + motor_timer_4 = timer_read (); + /* Position tracking. */ postrack_update (); + motor_timer_3 = timer_read (); + /* Position control. */ + if (motor_mode >= 2) + { + goto_update (); + } motor_timer_2 = timer_read (); /* Speed control. */ if (motor_mode >= 1) @@ -161,6 +170,29 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('z', 0): reset (); break; + case c ('g', 8): + motor_mode = 2; + goto_x = (uint32_t) args[0] << 24 | (uint32_t) args[1] << 16 + | args[2] << 8 | args[3]; + goto_y = (uint32_t) args[4] << 24 | (uint32_t) args[5] << 16 + | args[6] << 8 | args[7]; + break; + case c ('s', 0): + motor_mode = 1; + speed_left_aim = 0; + speed_right_aim = 0; + break; + case c ('s', 2): + motor_mode = 1; + speed_left_aim = args[0]; + speed_right_aim = args[1]; + break; + case c ('s', 3): + motor_mode = 1; + speed_left_aim = args[0]; + speed_right_aim = args[1]; + speed_max = args[2]; + break; case c ('w', 0): speed_restart (); motor_mode = 0; @@ -173,11 +205,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) pwm_left = args[0] << 8 | args[1]; pwm_right = args[2] << 8 | args[3]; break; - case c ('s', 2): - motor_mode = 1; - speed_left_aim = args[0]; - speed_right_aim = args[1]; - break; case c ('a', 1): speed_acc_cpt = speed_acc = args[0]; break; diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c index 54962b3..64450f9 100644 --- a/n/asserv/src/speed.c +++ b/n/asserv/src/speed.c @@ -27,6 +27,8 @@ int8_t speed_left, speed_right; /** Wanted speed. */ int8_t speed_left_aim, speed_right_aim; +/** Max speed. */ +int8_t speed_max = 0x20; /** Acceleration value. */ uint8_t speed_acc = 8; /** Acceleration counter, speed gets updated when it reachs 0. */ @@ -157,3 +159,37 @@ speed_restart (void) speed_left = 0; speed_right = 0; } + +/** Set maximum speed in order to be able to break before a distance and + * angle, inputs are f24.8 */ +static inline void +speed_distance (int32_t dist, int32_t arc) +{ + /* XXX: There are some buggy assertions there if dist and arc are both + * big. */ + uint8_t vls, vas; + uint32_t vl248, va248; + int8_t vl, va; + /* Drop sign. */ + vls = dist < 0; + if (vls) dist = -dist; + vas = arc < 0; + if (vas) arc = -arc; + /* Compute speed. + * v = sqrt (2 * a * d), a = 1/speed_acc */ + vl248 = dsp_sqrt (dist * 2 / speed_acc); + vl = (vl248 >> 8) & 0xff; + va248 = dsp_sqrt (arc * 2 / speed_acc); + va = (va248 >> 8) & 0xff; + /* Saturate. */ + if (vl248 & 0xffff0000 || vl > speed_max) + vl = speed_max; + if (va248 & 0xffff0000 || va > speed_max) + va = speed_max; + /* Get sign back. */ + if (vls) vl = -vl; + if (vas) va = -va; + speed_left_aim = vl - va; + speed_right_aim = vl + va; +} + -- cgit v1.2.3