summaryrefslogtreecommitdiff
path: root/digital/avr/modules/motor/speed_control
diff options
context:
space:
mode:
authorNicolas Schodet2011-10-08 11:56:10 +0200
committerNicolas Schodet2012-03-06 23:16:05 +0100
commit928dc853e088849580aac368401999ea596644bb (patch)
tree7970d54f08a9e802f54c7dbbde4f91987ea5982e /digital/avr/modules/motor/speed_control
parent7a1417eb3cecd3b5f5b8a5c1a73e5a89d19de2d4 (diff)
digital/avr/modules/motor: add motor control system
Diffstat (limited to 'digital/avr/modules/motor/speed_control')
-rw-r--r--digital/avr/modules/motor/speed_control/Makefile5
-rw-r--r--digital/avr/modules/motor/speed_control/Makefile.module1
-rw-r--r--digital/avr/modules/motor/speed_control/speed_control.c151
-rw-r--r--digital/avr/modules/motor/speed_control/speed_control.h91
-rw-r--r--digital/avr/modules/motor/speed_control/speed_control.txt39
5 files changed, 287 insertions, 0 deletions
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