/* 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]);
}