summaryrefslogtreecommitdiff
path: root/digital/asserv/src/asserv/speed.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/asserv/src/asserv/speed.c')
-rw-r--r--digital/asserv/src/asserv/speed.c165
1 files changed, 0 insertions, 165 deletions
diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c
deleted file mode 100644
index 32102177..00000000
--- a/digital/asserv/src/asserv/speed.c
+++ /dev/null
@@ -1,165 +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.
- */
-
-/** Theta/alpha speed control states. */
-struct speed_t speed_theta, speed_alpha;
-
-/** Auxiliaries speed control states. */
-struct speed_t speed_aux[AC_ASSERV_AUX_NB];
-
-/** Initialise speed control states. */
-void
-speed_init (void)
-{
- uint8_t i;
- speed_theta.pos = &pos_theta;
- speed_alpha.pos = &pos_alpha;
- 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 two motors system. */
-static void
-speed_update_double (struct state_t *state, struct speed_t *speed0,
- struct speed_t *speed1)
-{
- if (state->mode >= MODE_SPEED)
- {
- /* Update speed. */
- speed_update_by (speed0);
- speed_update_by (speed1);
- /* Check for completion. */
- if (state->mode == MODE_SPEED
- && (speed0->use_pos || speed1->use_pos)
- && (!speed0->use_pos || speed0->cur == 0)
- && (!speed1->use_pos || speed1->cur == 0))
- {
- state_finish (state);
- }
- }
-}
-
-/** 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;
- speed_update_double (&state_main, &speed_theta, &speed_alpha);
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
- speed_update_single (&state_aux[i], &speed_aux[i]);
-}
-