From df7d0d94ecc69fa9aadd2ab90db89b5ef1a74030 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 29 Apr 2010 23:03:56 +0200 Subject: digital/mimot/src/dirty: add copy/pasted firmware for mimot, refs #123 This is a quick and dirty firmware copied from asserv. This will be improved once #124 is closed. --- digital/mimot/src/dirty/speed.c | 130 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 130 insertions(+) create mode 100644 digital/mimot/src/dirty/speed.c (limited to 'digital/mimot/src/dirty/speed.c') diff --git a/digital/mimot/src/dirty/speed.c b/digital/mimot/src/dirty/speed.c new file mode 100644 index 00000000..f9d4193e --- /dev/null +++ b/digital/mimot/src/dirty/speed.c @@ -0,0 +1,130 @@ +/* speed.c - Speed control. */ +/* asserv - Position & speed motor control on AVR. {{{ + * + * Copyright (C) 2005 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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. + * + * }}} */ +#include "common.h" +#include "speed.h" + +#include "modules/utils/utils.h" +#include "modules/math/fixed/fixed.h" + +#include "pos.h" +#include "state.h" + +/** + * This file is responsible for speed control. It changes the current shafts + * positions using ramps. It can be controlled by a wanted speed or wanted + * shaft position. + */ + +/** Auxiliaries speed control states. */ +struct speed_t speed_aux[AC_ASSERV_AUX_NB]; + +/** Initialise speed control states. */ +void +speed_init (void) +{ + uint8_t i; + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + speed_aux[i].pos = &pos_aux[i]; +} + +/** Update current speed according to a speed consign. */ +static void +speed_update_by_speed (struct speed_t *speed) +{ + /* Update current speed. */ + if (UTILS_ABS (speed->cons - speed->cur) < speed->acc) + speed->cur = speed->cons; + else if (speed->cons > speed->cur) + speed->cur += speed->acc; + else + speed->cur -= speed->acc; +} + +/** Compute maximum allowed speed according to: distance left, maximum speed, + * current speed and acceleration. */ +static int16_t +speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max) +{ + int16_t s; + /* Compute maximum speed in order to be able to brake in time. + * The "+ 0xff" is to ceil result. + * s = sqrt (2 * a * d) */ + s = fixed_sqrt_ui32 ((2 * UTILS_ABS (d) * acc + 0xff) >> 8); + /* Apply consign. */ + s = UTILS_MIN (max, s); + /* Apply sign. */ + if (d < 0) + s = -s; + /* Convert to f8.8 and check acceleration. */ + s = s << 8; + UTILS_BOUND (s, cur - acc, cur + acc); + return s; +} + +/** Update current speed according to a position consign. */ +static void +speed_update_by_position (struct speed_t *speed) +{ + int32_t diff = speed->pos_cons - speed->pos->cons; + speed->cur = speed_compute_max_speed (diff, speed->cur, speed->acc, + speed->max); +} + +/** Update shaft position consign according to its consign type. */ +static void +speed_update_by (struct speed_t *speed) +{ + if (speed->use_pos) + speed_update_by_position (speed); + else + speed_update_by_speed (speed); + /* Update shaft position. */ + speed->pos->cons += speed->cur >> 8; +} + +/** Update shaft position consign for one motor system. */ +static void +speed_update_single (struct state_t *state, struct speed_t *speed) +{ + if (state->mode >= MODE_SPEED) + { + /* Update speed. */ + speed_update_by (speed); + /* Check for completion. */ + if (state->mode == MODE_SPEED + && speed->use_pos && speed->cur == 0) + state_finish (state); + } +} + +/** Update shaft position consign according to consign. */ +void +speed_update (void) +{ + uint8_t i; + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + speed_update_single (&state_aux[i], &speed_aux[i]); +} + -- cgit v1.2.3