From 830048da9a94cd9fb91678694dfd07d577e3070c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 5 Mar 2012 23:49:17 +0100 Subject: digital/mimot: convert to new control system --- digital/mimot/src/dirty/speed.c | 138 ---------------------------------------- 1 file changed, 138 deletions(-) delete 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 deleted file mode 100644 index 80ca3d6b..00000000 --- a/digital/mimot/src/dirty/speed.c +++ /dev/null @@ -1,138 +0,0 @@ -/* 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 (be careful of overflow!). */ - if (speed->cons > speed->cur) - { - if ((uint16_t) (speed->cons - speed->cur) < (uint16_t) speed->acc) - speed->cur = speed->cons; - else - speed->cur += speed->acc; - } - else - { - if ((uint16_t) (speed->cur - speed->cons) < (uint16_t) speed->acc) - speed->cur = speed->cons; - 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