From 928dc853e088849580aac368401999ea596644bb Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 8 Oct 2011 11:56:10 +0200 Subject: digital/avr/modules/motor: add motor control system --- digital/avr/modules/motor/speed_control/Makefile | 5 + .../modules/motor/speed_control/Makefile.module | 1 + .../modules/motor/speed_control/speed_control.c | 151 +++++++++++++++++++++ .../modules/motor/speed_control/speed_control.h | 91 +++++++++++++ .../modules/motor/speed_control/speed_control.txt | 39 ++++++ 5 files changed, 287 insertions(+) create mode 100644 digital/avr/modules/motor/speed_control/Makefile create mode 100644 digital/avr/modules/motor/speed_control/Makefile.module create mode 100644 digital/avr/modules/motor/speed_control/speed_control.c create mode 100644 digital/avr/modules/motor/speed_control/speed_control.h create mode 100644 digital/avr/modules/motor/speed_control/speed_control.txt (limited to 'digital/avr/modules/motor/speed_control') diff --git a/digital/avr/modules/motor/speed_control/Makefile b/digital/avr/modules/motor/speed_control/Makefile new file mode 100644 index 00000000..dd706c6d --- /dev/null +++ b/digital/avr/modules/motor/speed_control/Makefile @@ -0,0 +1,5 @@ +BASE = ../../.. +DOC = speed_control.html +EXTRACTDOC = speed_control.h + +include $(BASE)/make/Makefile.gen diff --git a/digital/avr/modules/motor/speed_control/Makefile.module b/digital/avr/modules/motor/speed_control/Makefile.module new file mode 100644 index 00000000..b6224c7e --- /dev/null +++ b/digital/avr/modules/motor/speed_control/Makefile.module @@ -0,0 +1 @@ +motor_speed_control_SOURCES = speed_control.c diff --git a/digital/avr/modules/motor/speed_control/speed_control.c b/digital/avr/modules/motor/speed_control/speed_control.c new file mode 100644 index 00000000..2caf0937 --- /dev/null +++ b/digital/avr/modules/motor/speed_control/speed_control.c @@ -0,0 +1,151 @@ +/* speed_control.c */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2011 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_control.h" + +#include "modules/utils/utils.h" +#include "modules/math/fixed/fixed.h" + +/** Update current speed according to a speed consign. */ +static void +speed_control_update_by_speed (speed_control_t *speed_control) +{ + /* Update current speed (be careful of overflow!). */ + if (speed_control->cons > speed_control->cur) + { + if ((uint16_t) (speed_control->cons - speed_control->cur) + < (uint16_t) speed_control->acc) + speed_control->cur = speed_control->cons; + else + speed_control->cur += speed_control->acc; + } + else + { + if ((uint16_t) (speed_control->cur - speed_control->cons) + < (uint16_t) speed_control->acc) + speed_control->cur = speed_control->cons; + else + speed_control->cur -= speed_control->acc; + } +} + +/** Compute maximum allowed speed according to: distance left, maximum speed, + * current speed and acceleration. */ +static int16_t +speed_control_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_control_update_by_position (speed_control_t *speed_control, + pos_control_t *pos_control) +{ + int32_t diff = speed_control->pos_cons - pos_control->cons; + speed_control->cur = speed_control_compute_max_speed + (diff, speed_control->cur, speed_control->acc, speed_control->max); +} + +void +speed_control_init (speed_control_t *speed_control, + pos_control_t *pos_control) +{ + speed_control->pos_control = pos_control; +} + +void +speed_control_update (speed_control_t *speed_control, + uint8_t enabled) +{ + if (enabled) + { + /* Compute speed. */ + if (speed_control->use_pos) + speed_control_update_by_position (speed_control, + speed_control->pos_control); + else + speed_control_update_by_speed (speed_control); + /* Update shaft position. */ + speed_control->pos_control->cons += speed_control->cur >> 8; + } + else + speed_control->cur = 0; +} + +void +speed_control_set_speed (speed_control_t *speed_control, int8_t speed) +{ + speed_control->use_pos = 0; + speed_control->cons = speed << 8; +} + +void +speed_control_pos_offset (speed_control_t *speed_control, int32_t offset) +{ + speed_control->use_pos = 1; + speed_control->pos_cons = speed_control->pos_control->cons + offset; +} + +void +speed_control_pos_offset_from_here (speed_control_t *speed_control, + int32_t offset) +{ + speed_control->use_pos = 1; + speed_control->pos_cons = speed_control->pos_control->cur + offset; +} + +void +speed_control_pos (speed_control_t *speed_control, int32_t pos_cons) +{ + speed_control->use_pos = 1; + speed_control->pos_cons = pos_cons; +} + +void +speed_control_hard_stop (speed_control_t *speed_control) +{ + /* No future movement. */ + speed_control->use_pos = 0; + speed_control->cur = 0; + speed_control->cons = 0; + /* Really stop right here, position control on the current point. */ + speed_control->pos_control->cons = speed_control->pos_control->cur; +} + diff --git a/digital/avr/modules/motor/speed_control/speed_control.h b/digital/avr/modules/motor/speed_control/speed_control.h new file mode 100644 index 00000000..14e1ef15 --- /dev/null +++ b/digital/avr/modules/motor/speed_control/speed_control.h @@ -0,0 +1,91 @@ +#ifndef speed_control_h +#define speed_control_h +/* speed_control.h */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2011 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 "modules/motor/pos_control/pos_control.h" + +/** Speed control state. */ +struct speed_control_t +{ + /** Current speed, f8.8. */ + int16_t cur; + /** Whether to use the consign position (1) or not (0). */ + uint8_t use_pos; + /** Consign speed, f8.8. */ + int16_t cons; + /** Consign position. */ + uint32_t pos_cons; + /** Maximum speed for position consign, u7. */ + int8_t max; + /** Slow speed, deprecated, u7. */ + int8_t slow; + /** Acceleration, f8.8. */ + int16_t acc; + /** Associated position control, to simplify function prototypes. */ + pos_control_t *pos_control; +}; +typedef struct speed_control_t speed_control_t; + +/** Initialise speed control. */ +void +speed_control_init (speed_control_t *speed_control, + pos_control_t *pos_control); + +/** Control speed and update position control. */ +void +speed_control_update (speed_control_t *speed_control, + uint8_t enabled); + +/** Set speed consign. Accelerate to the given speed. */ +void +speed_control_set_speed (speed_control_t *speed_control, int8_t speed); + +/** Set position consign offset. Move to a position measured from the current + * controlled position (which may be different from the actual position), + * control speed to match requested acceleration. */ +void +speed_control_pos_offset (speed_control_t *speed_control, int32_t offset); + +/** Set position consign offset from the current actual position. Move to a + * position measured from the actual position, control speed to match + * requested acceleration. + * + * Warning: This is usually not what you want. Only use this if offset has + * been computed from encoder. */ +void +speed_control_pos_offset_from_here (speed_control_t *speed_control, + int32_t offset); + +/** Set absolute position consign. Useful to restore an previous position. */ +void +speed_control_pos (speed_control_t *speed_control, int32_t pos_cons); + +/** Stop right now, no acceleration! You really should avoid this function + * unless the motor is actually almost stopped. */ +void +speed_control_hard_stop (speed_control_t *speed_control); + +#endif /* speed_control_h */ diff --git a/digital/avr/modules/motor/speed_control/speed_control.txt b/digital/avr/modules/motor/speed_control/speed_control.txt new file mode 100644 index 00000000..94b4d0b4 --- /dev/null +++ b/digital/avr/modules/motor/speed_control/speed_control.txt @@ -0,0 +1,39 @@ +============================ + motor/speed_control module +============================ +:Author: Nicolas Schodet + +Introduction +============ + +This module provides two speed control modes, to be chained with a position +control module. Its role is to make sure that movement acceleration is not +too high. + +Usage +===== + +The `max` and `acc` parameters should be set to maximum speed and maximum +acceleration. Other fields should better be changed using the helper +functions. + +The first control mode respects a speed consign. User choose a desired speed +and the module will change the actual speed according to acceleration to match +consign. Use the `speed_control_set_speed`. + +The second mode will move to a position, by increasing and decreasing speed +according to the acceleration. Use the `speed_control_pos_offset` and +`speed_control_pos_offset_from_here` functions. The first function use an +offset relative to the current controlled position so that small errors are not +accumulated over several movements. + +The `speed_control_hard_stop` function is quite dangerous as it does not +respect the maximum acceleration. Only use this if you know what you are +doing! This can be used for example to stop as soon as a contact is triggered +to find a zero position at very low speed. The regular braking method is +`speed_control_set_speed` with a zero speed. + +API +=== + +.. include:: speed_control.exd -- cgit v1.2.3