summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2011-10-08 11:56:10 +0200
committerNicolas Schodet2012-03-06 23:16:05 +0100
commit928dc853e088849580aac368401999ea596644bb (patch)
tree7970d54f08a9e802f54c7dbbde4f91987ea5982e
parent7a1417eb3cecd3b5f5b8a5c1a73e5a89d19de2d4 (diff)
digital/avr/modules/motor: add motor control system
-rw-r--r--digital/avr/modules/motor/blocking_detection/Makefile5
-rw-r--r--digital/avr/modules/motor/blocking_detection/Makefile.module1
-rw-r--r--digital/avr/modules/motor/blocking_detection/blocking_detection.c54
-rw-r--r--digital/avr/modules/motor/blocking_detection/blocking_detection.h53
-rw-r--r--digital/avr/modules/motor/blocking_detection/blocking_detection.txt21
-rw-r--r--digital/avr/modules/motor/control_state/Makefile5
-rw-r--r--digital/avr/modules/motor/control_state/Makefile.module1
-rw-r--r--digital/avr/modules/motor/control_state/control_state.h111
-rw-r--r--digital/avr/modules/motor/control_state/control_state.txt47
-rw-r--r--digital/avr/modules/motor/control_system/Makefile5
-rw-r--r--digital/avr/modules/motor/control_system/Makefile.module1
-rw-r--r--digital/avr/modules/motor/control_system/control_system.c142
-rw-r--r--digital/avr/modules/motor/control_system/control_system.h92
-rw-r--r--digital/avr/modules/motor/control_system/control_system.txt33
-rw-r--r--digital/avr/modules/motor/pos_control/Makefile5
-rw-r--r--digital/avr/modules/motor/pos_control/Makefile.module1
-rw-r--r--digital/avr/modules/motor/pos_control/pos_control.c103
-rw-r--r--digital/avr/modules/motor/pos_control/pos_control.h60
-rw-r--r--digital/avr/modules/motor/pos_control/pos_control.txt32
-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
24 files changed, 1059 insertions, 0 deletions
diff --git a/digital/avr/modules/motor/blocking_detection/Makefile b/digital/avr/modules/motor/blocking_detection/Makefile
new file mode 100644
index 00000000..22849574
--- /dev/null
+++ b/digital/avr/modules/motor/blocking_detection/Makefile
@@ -0,0 +1,5 @@
+BASE = ../../..
+DOC = blocking_detection.html
+EXTRACTDOC = blocking_detection.h
+
+include $(BASE)/make/Makefile.gen
diff --git a/digital/avr/modules/motor/blocking_detection/Makefile.module b/digital/avr/modules/motor/blocking_detection/Makefile.module
new file mode 100644
index 00000000..155485c4
--- /dev/null
+++ b/digital/avr/modules/motor/blocking_detection/Makefile.module
@@ -0,0 +1 @@
+motor_blocking_detection_SOURCES = blocking_detection.c
diff --git a/digital/avr/modules/motor/blocking_detection/blocking_detection.c b/digital/avr/modules/motor/blocking_detection/blocking_detection.c
new file mode 100644
index 00000000..5cac8ab4
--- /dev/null
+++ b/digital/avr/modules/motor/blocking_detection/blocking_detection.c
@@ -0,0 +1,54 @@
+/* blocking_detection.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 "blocking_detection.h"
+
+#include "modules/utils/utils.h"
+
+void
+blocking_detection_update (blocking_detection_t *bd,
+ int16_t current_speed, int16_t output,
+ pos_control_t *pos_control,
+ uint8_t control_enabled)
+{
+ if (control_enabled)
+ {
+ /* Test for blocking condition. */
+ int32_t error = pos_control->cons - pos_control->cur;
+ if (UTILS_ABS (error) > bd->error_limit
+ && UTILS_ABS (current_speed) < bd->speed_limit)
+ bd->counter++;
+ else
+ bd->counter = 0;
+ /* Set blocked state for detector clients. */
+ bd->blocked = bd->counter > bd->counter_limit;
+ }
+ else
+ {
+ bd->blocked = 0;
+ bd->counter = 0;
+ }
+}
+
diff --git a/digital/avr/modules/motor/blocking_detection/blocking_detection.h b/digital/avr/modules/motor/blocking_detection/blocking_detection.h
new file mode 100644
index 00000000..61fa6e04
--- /dev/null
+++ b/digital/avr/modules/motor/blocking_detection/blocking_detection.h
@@ -0,0 +1,53 @@
+#ifndef blocking_detection_h
+#define blocking_detection_h
+/* blocking_detection.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"
+
+/** Blocking detection state. */
+struct blocking_detection_t
+{
+ /** True if blocking is detected. */
+ uint8_t blocked;
+ /** Count the number of blocked detection. */
+ uint8_t counter;
+ /** Error limit. */
+ int32_t error_limit;
+ /** Speed limit. */
+ int16_t speed_limit;
+ /** Counter limit. */
+ uint8_t counter_limit;
+};
+typedef struct blocking_detection_t blocking_detection_t;
+
+/** Detect blocking, update blocking state. */
+void
+blocking_detection_update (blocking_detection_t *blocking_detection,
+ int16_t current_speed, int16_t output,
+ pos_control_t *pos_control,
+ uint8_t control_enabled);
+
+#endif /* blocking_detection_h */
diff --git a/digital/avr/modules/motor/blocking_detection/blocking_detection.txt b/digital/avr/modules/motor/blocking_detection/blocking_detection.txt
new file mode 100644
index 00000000..21319126
--- /dev/null
+++ b/digital/avr/modules/motor/blocking_detection/blocking_detection.txt
@@ -0,0 +1,21 @@
+=================================
+ motor/blocking_detection module
+=================================
+:Author: Nicolas Schodet
+
+Introduction
+============
+
+This module try to detect when there is something which blocks a motor. This
+is not meant to protect the motor, its role is to inform upper layer that an
+action must be taken.
+
+Usage
+=====
+
+TODO: about to change.
+
+API
+===
+
+.. include:: blocking_detection.exd
diff --git a/digital/avr/modules/motor/control_state/Makefile b/digital/avr/modules/motor/control_state/Makefile
new file mode 100644
index 00000000..c8d57002
--- /dev/null
+++ b/digital/avr/modules/motor/control_state/Makefile
@@ -0,0 +1,5 @@
+BASE = ../../..
+DOC = control_state.html
+EXTRACTDOC = control_state.h
+
+include $(BASE)/make/Makefile.gen
diff --git a/digital/avr/modules/motor/control_state/Makefile.module b/digital/avr/modules/motor/control_state/Makefile.module
new file mode 100644
index 00000000..dd253dd8
--- /dev/null
+++ b/digital/avr/modules/motor/control_state/Makefile.module
@@ -0,0 +1 @@
+motor_control_state_SOURCES =
diff --git a/digital/avr/modules/motor/control_state/control_state.h b/digital/avr/modules/motor/control_state/control_state.h
new file mode 100644
index 00000000..aa2a1345
--- /dev/null
+++ b/digital/avr/modules/motor/control_state/control_state.h
@@ -0,0 +1,111 @@
+#ifndef control_state_h
+#define control_state_h
+/* control_state.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.
+ *
+ * }}} */
+
+/** Control modes. */
+enum control_state_mode_t
+{
+ /** No control, direct output. */
+ CS_MODE_NONE = 0,
+ /** Blocking detection, should only be disabled when a higher layer take
+ * care of it. Disabling it will only disable its effect, blocking
+ * detection is still done, but not applied. */
+ CS_MODE_BLOCKING_DETECTION = 1,
+ /** Position control for linear movement on a polar system. */
+ CS_MODE_POS_CONTROL_THETA = 2,
+ /** Position control for angular movement on a polar system. */
+ CS_MODE_POS_CONTROL_ALPHA = 4,
+ /** Position control for single motor system, can also be used with
+ * control_state_set_mode to enable both controls on a polar system.
+ * If position control is disabled, output should be assigned a value or
+ * else it will keep its last computed value. */
+ CS_MODE_POS_CONTROL = 4,
+ /** Speed control, for speed consign or speed limited position consign. */
+ CS_MODE_SPEED_CONTROL = 8,
+ /** Trajectory control, higher level control. */
+ CS_MODE_TRAJ_CONTROL = 16,
+ /** Finished flag, set when a high level order is completed. */
+ CS_MODE_FINISHED = 64,
+ /** Blocked, output tied to zero. */
+ CS_MODE_BLOCKED = 128,
+};
+typedef enum control_state_mode_t control_state_mode_t;
+
+/** Control state. */
+struct control_state_t
+{
+ /** Mask of currently selected control modes. */
+ uint8_t modes;
+};
+typedef struct control_state_t control_state_t;
+
+/** Set current modes.
+ * - enable: enable all mode lower or equal to this mode.
+ * - disable_mask: if non zero, mask of modes to disable.
+ */
+static inline void
+control_state_set_mode (control_state_t *control_state, uint8_t enable,
+ uint8_t disable_mask)
+{
+ uint8_t enable_mask = enable ? (enable | (enable - 1)) : 0;
+ control_state->modes = enable_mask & ~disable_mask;
+}
+
+/** Set finished flag, restore blocking detection and full position control if
+ * any position control is enabled. */
+static inline void
+control_state_finished (control_state_t *control_state)
+{
+ control_state->modes |= CS_MODE_FINISHED;
+ if (control_state->modes & (CS_MODE_POS_CONTROL_THETA
+ | CS_MODE_POS_CONTROL_ALPHA))
+ control_state->modes |= (CS_MODE_POS_CONTROL_THETA
+ | CS_MODE_POS_CONTROL_ALPHA
+ | CS_MODE_BLOCKING_DETECTION);
+}
+
+/** Set blocked state, disable all other modes. */
+static inline void
+control_state_blocked (control_state_t *control_state)
+{
+ control_state->modes = CS_MODE_BLOCKED;
+}
+
+/** Return non zero if finished. */
+static inline uint8_t
+control_state_is_finished (control_state_t *control_state)
+{
+ return control_state->modes & CS_MODE_FINISHED;
+}
+
+/** Return non zero if blocked. */
+static inline uint8_t
+control_state_is_blocked (control_state_t *control_state)
+{
+ return control_state->modes & CS_MODE_BLOCKED;
+}
+
+#endif /* control_state_h */
diff --git a/digital/avr/modules/motor/control_state/control_state.txt b/digital/avr/modules/motor/control_state/control_state.txt
new file mode 100644
index 00000000..68a465ba
--- /dev/null
+++ b/digital/avr/modules/motor/control_state/control_state.txt
@@ -0,0 +1,47 @@
+============================
+ motor/control_state module
+============================
+:Author: Nicolas Schodet
+
+Introduction
+============
+
+The control state provides a way to enable or disable each control module in a
+control system.
+
+Usage
+=====
+
+Use `control_state_set_mode` to enable a control mode and every lower control
+mode. For example::
+
+ control_state_set_mode (CS_MODE_SPEED_CONTROL, 0);
+
+will enable speed control, position control and blocking detection.
+
+The second argument can be used to disable a lower mode. For example, you can
+enable position control without blocking detection::
+
+ control_state_set_mode (CS_MODE_POS_CONTROL, CS_MODE_BLOCKING_DETECTION);
+
+Actually, blocking detection is always enabled, only its effect can be
+avoided (if disable, a blocked state will not disable control).
+
+Use `control_state_finished` to signal that the current high level move is
+finished. This will restore every position control and blocking detection if
+at least one position control is enabled and set the finished flag.
+
+Use `control_state_blocked` to signal a blocked state. This will disable
+every mode and set the blocked flag.
+
+The functions `control_state_is_finished` and `control_state_is_blocked` can
+be used to query those flags.
+
+When state has changed, it is usually safest to wait for the next control
+cycle before state is changed again. This is done naturally if the order of
+update functions is respected (see control system documentation).
+
+API
+===
+
+.. include:: control_state.exd
diff --git a/digital/avr/modules/motor/control_system/Makefile b/digital/avr/modules/motor/control_system/Makefile
new file mode 100644
index 00000000..b4a8d25a
--- /dev/null
+++ b/digital/avr/modules/motor/control_system/Makefile
@@ -0,0 +1,5 @@
+BASE = ../../..
+DOC = control_system.html
+EXTRACTDOC = control_system.h
+
+include $(BASE)/make/Makefile.gen
diff --git a/digital/avr/modules/motor/control_system/Makefile.module b/digital/avr/modules/motor/control_system/Makefile.module
new file mode 100644
index 00000000..05871b24
--- /dev/null
+++ b/digital/avr/modules/motor/control_system/Makefile.module
@@ -0,0 +1 @@
+motor_control_system_SOURCES = control_system.c
diff --git a/digital/avr/modules/motor/control_system/control_system.c b/digital/avr/modules/motor/control_system/control_system.c
new file mode 100644
index 00000000..96f0cb58
--- /dev/null
+++ b/digital/avr/modules/motor/control_system/control_system.c
@@ -0,0 +1,142 @@
+/* control_system.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 "control_system.h"
+
+void
+control_system_single_init (control_system_single_t *cs)
+{
+ speed_control_init (&cs->speed, &cs->pos);
+ pos_control_init (&cs->pos);
+}
+
+void
+control_system_polar_init (control_system_polar_t *cs)
+{
+ speed_control_init (&cs->speed_theta, &cs->pos_theta);
+ speed_control_init (&cs->speed_alpha, &cs->pos_alpha);
+ pos_control_init (&cs->pos_theta);
+ pos_control_init (&cs->pos_alpha);
+}
+
+void
+control_system_single_update_prepare (control_system_single_t *cs)
+{
+ uint8_t control_modes = cs->state.modes;
+ /* Update speed control. */
+ speed_control_update (&cs->speed, control_modes & CS_MODE_SPEED_CONTROL);
+}
+
+void
+control_system_polar_update_prepare (control_system_polar_t *cs)
+{
+ uint8_t control_modes = cs->state.modes;
+ /* Update speed control. */
+ speed_control_update (&cs->speed_theta,
+ control_modes & CS_MODE_SPEED_CONTROL);
+ speed_control_update (&cs->speed_alpha,
+ control_modes & CS_MODE_SPEED_CONTROL);
+}
+
+void
+control_system_single_update (control_system_single_t *cs)
+{
+ uint8_t control_modes = cs->state.modes;
+ int16_t current_speed;
+ int16_t out;
+ /* Update pos control. */
+ current_speed = cs->encoder->diff;
+ out = pos_control_update (&cs->pos, current_speed,
+ control_modes & CS_MODE_POS_CONTROL);
+ /* Test for blocking condition, detection is done if control is enabled,
+ * but action is taken depending on the blocking detection mode. */
+ blocking_detection_update (&cs->blocking_detection, current_speed, out,
+ &cs->pos, control_modes & CS_MODE_POS_CONTROL);
+ if (control_modes & CS_MODE_BLOCKING_DETECTION
+ && cs->blocking_detection.blocked)
+ {
+ control_state_blocked (&cs->state);
+ /* Reset control so that another action can be done immediately. */
+ pos_control_update (&cs->pos, 0, 0);
+ blocking_detection_update (&cs->blocking_detection, 0, 0,
+ &cs->pos, 0);
+ }
+ /* Set output value if under position control, zero on blocking. */
+ if (control_modes & CS_MODE_POS_CONTROL)
+ output_set (cs->output, out);
+ else if (control_modes & CS_MODE_BLOCKED)
+ output_set (cs->output, 0);
+}
+
+void
+control_system_polar_update (control_system_polar_t *cs)
+{
+ uint8_t control_modes = cs->state.modes;
+ int16_t current_speed_theta, current_speed_alpha;
+ int16_t out_theta, out_alpha;
+ /* Update pos control. */
+ current_speed_theta = cs->encoder_right->diff + cs->encoder_left->diff;
+ current_speed_alpha = cs->encoder_right->diff - cs->encoder_left->diff;
+ out_theta = pos_control_update
+ (&cs->pos_theta, current_speed_theta,
+ control_modes & CS_MODE_POS_CONTROL_THETA);
+ out_alpha = pos_control_update
+ (&cs->pos_alpha, current_speed_alpha,
+ control_modes & CS_MODE_POS_CONTROL_ALPHA);
+ /* Test for blocking condition, detection is done if control is enabled,
+ * but action is taken depending on the blocking detection mode. */
+ blocking_detection_update (&cs->blocking_detection_theta,
+ current_speed_theta, out_theta, &cs->pos_theta,
+ control_modes & CS_MODE_POS_CONTROL_THETA);
+ blocking_detection_update (&cs->blocking_detection_alpha,
+ current_speed_alpha, out_alpha, &cs->pos_alpha,
+ control_modes & CS_MODE_POS_CONTROL_ALPHA);
+ if (control_modes & CS_MODE_BLOCKING_DETECTION
+ && (cs->blocking_detection_theta.blocked
+ || cs->blocking_detection_alpha.blocked))
+ {
+ control_state_blocked (&cs->state);
+ /* Reset control so that another action can be done immediately. */
+ pos_control_update (&cs->pos_theta, 0, 0);
+ pos_control_update (&cs->pos_alpha, 0, 0);
+ blocking_detection_update (&cs->blocking_detection_theta, 0, 0,
+ &cs->pos_theta, 0);
+ blocking_detection_update (&cs->blocking_detection_alpha, 0, 0,
+ &cs->pos_alpha, 0);
+ }
+ /* Set output value if under position control, zero on blocking. */
+ if (control_modes & (CS_MODE_POS_CONTROL_THETA |
+ CS_MODE_POS_CONTROL_ALPHA))
+ {
+ output_set (cs->output_left, out_theta - out_alpha);
+ output_set (cs->output_right, out_theta + out_alpha);
+ }
+ else if (control_modes & CS_MODE_BLOCKED)
+ {
+ output_set (cs->output_left, 0);
+ output_set (cs->output_right, 0);
+ }
+}
+
diff --git a/digital/avr/modules/motor/control_system/control_system.h b/digital/avr/modules/motor/control_system/control_system.h
new file mode 100644
index 00000000..e8e6a434
--- /dev/null
+++ b/digital/avr/modules/motor/control_system/control_system.h
@@ -0,0 +1,92 @@
+#ifndef control_system_h
+#define control_system_h
+/* control_system.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/control_state/control_state.h"
+#include "modules/motor/encoder/encoder.h"
+#include "modules/motor/speed_control/speed_control.h"
+#include "modules/motor/pos_control/pos_control.h"
+#include "modules/motor/blocking_detection/blocking_detection.h"
+#include "modules/motor/output/output.h"
+
+/** Single motor control system. */
+struct control_system_single_t
+{
+ control_state_t state;
+ encoder_t *encoder;
+ speed_control_t speed;
+ pos_control_t pos;
+ blocking_detection_t blocking_detection;
+ output_t *output;
+};
+typedef struct control_system_single_t control_system_single_t;
+
+/** Polar control system. */
+struct control_system_polar_t
+{
+ control_state_t state;
+ encoder_t *encoder_left;
+ encoder_t *encoder_right;
+ speed_control_t speed_theta;
+ speed_control_t speed_alpha;
+ pos_control_t pos_theta;
+ pos_control_t pos_alpha;
+ blocking_detection_t blocking_detection_theta;
+ blocking_detection_t blocking_detection_alpha;
+ output_t *output_left;
+ output_t *output_right;
+};
+typedef struct control_system_polar_t control_system_polar_t;
+
+/** Initialise single motor control system components. */
+void
+control_system_single_init (control_system_single_t *cs);
+
+/** Initialise polar control system components. */
+void
+control_system_polar_init (control_system_polar_t *cs);
+
+/** Prepare single motor control system update (update all non urgent
+ * modules). To be called before encoder is updated. */
+void
+control_system_single_update_prepare (control_system_single_t *cs);
+
+/** Prepare polar control system update (update all non urgent modules). To
+ * be called before encoders are updated. */
+void
+control_system_polar_update_prepare (control_system_polar_t *cs);
+
+/** Update single motor control system components (however do not update
+ * encoder, neither output). To be called after encoder is updated. */
+void
+control_system_single_update (control_system_single_t *cs);
+
+/** Update polar control system components (however do not update encoder,
+ * neither output). To be called after encoders are updated. */
+void
+control_system_polar_update (control_system_polar_t *cs);
+
+#endif /* control_system_h */
diff --git a/digital/avr/modules/motor/control_system/control_system.txt b/digital/avr/modules/motor/control_system/control_system.txt
new file mode 100644
index 00000000..e45db230
--- /dev/null
+++ b/digital/avr/modules/motor/control_system/control_system.txt
@@ -0,0 +1,33 @@
+=============================
+ motor/control_system module
+=============================
+:Author: Nicolas Schodet
+
+Introduction
+============
+
+This module will bind together several layers of motor control modules. It
+aims at helping sequencing them in the right order to meets real time
+constrains and state changes sequence required order.
+
+Usage
+=====
+
+You should use one of `control_system_single_t` for a single motor system, or
+`control_system_polar_t` for a dual motor differential robot configuration.
+The encoder and output structures should be initialised and their pointers be
+set before the `control_system_*_init` is called.
+
+The `control_system_*_update_prepare` function should be called right before
+you wait for the next cycle, but after you made changes to consigns. Once the
+next cycle start, as fast as possible, you should first update encoders, then
+call the `control_system_*_update` function, and finally update outputs.
+
+This is important that consigns (and control state) are not changed between
+update preparation and update, because the update function is also responsible
+to reset some internal variables when state is changed.
+
+API
+===
+
+.. include:: control_system.exd
diff --git a/digital/avr/modules/motor/pos_control/Makefile b/digital/avr/modules/motor/pos_control/Makefile
new file mode 100644
index 00000000..439f8adc
--- /dev/null
+++ b/digital/avr/modules/motor/pos_control/Makefile
@@ -0,0 +1,5 @@
+BASE = ../../..
+DOC = pos_control.html
+EXTRACTDOC = pos_control.h
+
+include $(BASE)/make/Makefile.gen
diff --git a/digital/avr/modules/motor/pos_control/Makefile.module b/digital/avr/modules/motor/pos_control/Makefile.module
new file mode 100644
index 00000000..cc290a21
--- /dev/null
+++ b/digital/avr/modules/motor/pos_control/Makefile.module
@@ -0,0 +1 @@
+motor_pos_control_SOURCES = pos_control.c
diff --git a/digital/avr/modules/motor/pos_control/pos_control.c b/digital/avr/modules/motor/pos_control/pos_control.c
new file mode 100644
index 00000000..80caae74
--- /dev/null
+++ b/digital/avr/modules/motor/pos_control/pos_control.c
@@ -0,0 +1,103 @@
+/* pos_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 "pos_control.h"
+
+#include "modules/utils/utils.h"
+
+/** Compute a PID.
+ * How to compute maximum numbers size:
+ * Result is 24 bits (16 bits kept after shift).
+ * If e_sat == 1023, e max is 11 bits (do not forget the sign bit), and diff
+ * max is 12 bits (can be saturated with d_sat).
+ * If i_sat == 1023, i max is 11 bits.
+ * In the final addition, let's give 23 bits to the p part, and 22 bits to the
+ * i and d part (23b + 22b + 22b => 23b + 23b => 24b).
+ * Therefore, kp can be 23 - 11 = 12 bits (f4.8).
+ * ki can be 22 - 11 = 11 bits (f3.8).
+ * kd can be 22 - 12 = 10 bits (f2.8).
+ * How to increase this number:
+ * - lower the shift.
+ * - bound the value returned.
+ * - lower e, i & d saturation. */
+static inline int16_t
+pos_control_compute_pid (struct pos_control_t *pos_control, int32_t e)
+{
+ int32_t diff, pid;
+ /* Saturate error. */
+ UTILS_BOUND (e, -pos_control->e_sat, pos_control->e_sat);
+ /* Integral update. */
+ pos_control->i += e;
+ UTILS_BOUND (pos_control->i, -pos_control->i_sat, pos_control->i_sat);
+ /* Differential value. */
+ diff = e - pos_control->last_error;
+ UTILS_BOUND (diff, -pos_control->d_sat, pos_control->d_sat);
+ /* Compute PID. */
+ pid = e * pos_control->kp + pos_control->i * pos_control->ki
+ + diff * pos_control->kd;
+ /* Save result. */
+ pos_control->last_error = e;
+ return pid >> 8;
+}
+
+/** Reset position control so that it does not go crazy when enabled. */
+static void
+pos_control_reset (pos_control_t *pos_control)
+{
+ pos_control->cons = pos_control->cur;
+ pos_control->i = 0;
+ pos_control->last_error = 0;
+}
+
+void
+pos_control_init (pos_control_t *pos_control)
+{
+ pos_control_reset (pos_control);
+}
+
+int16_t
+pos_control_update (pos_control_t *pos_control, int16_t cur_speed,
+ uint8_t enabled)
+{
+ int32_t error;
+ int16_t pid;
+ /* Update current shaft position. */
+ pos_control->cur += cur_speed;
+ /* Control position if enabled. */
+ if (enabled)
+ {
+ /* Compute error. */
+ error = pos_control->cons - pos_control->cur;
+ /* Compute PID. */
+ pid = pos_control_compute_pid (pos_control, error);
+ }
+ else
+ {
+ pos_control_reset (pos_control);
+ pid = 0;
+ }
+ return pid;
+}
+
diff --git a/digital/avr/modules/motor/pos_control/pos_control.h b/digital/avr/modules/motor/pos_control/pos_control.h
new file mode 100644
index 00000000..1378f3ad
--- /dev/null
+++ b/digital/avr/modules/motor/pos_control/pos_control.h
@@ -0,0 +1,60 @@
+#ifndef pos_control_h
+#define pos_control_h
+/* pos_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.
+ *
+ * }}} */
+
+/** Position control state. */
+struct pos_control_t
+{
+ /** Current position. */
+ uint32_t cur;
+ /** Consign position. */
+ uint32_t cons;
+ /** PID coefficients (f8.8, maximum depends on saturation values). */
+ uint16_t kp, ki, kd;
+ /** Error saturation. */
+ int32_t e_sat;
+ /** Integral saturation. */
+ int32_t i_sat;
+ /** Differential saturation. */
+ int32_t d_sat;
+ /** Current integral value. */
+ int32_t i;
+ /** Last error value. */
+ int32_t last_error;
+};
+typedef struct pos_control_t pos_control_t;
+
+/** Initialise position control. */
+void
+pos_control_init (pos_control_t *pos_control);
+
+/** Control position of system, take current speed as input, return the new
+ * command. */
+int16_t
+pos_control_update (pos_control_t *pos_control, int16_t cur_speed,
+ uint8_t enabled);
+
+#endif /* pos_control_h */
diff --git a/digital/avr/modules/motor/pos_control/pos_control.txt b/digital/avr/modules/motor/pos_control/pos_control.txt
new file mode 100644
index 00000000..d1c1d03f
--- /dev/null
+++ b/digital/avr/modules/motor/pos_control/pos_control.txt
@@ -0,0 +1,32 @@
+==========================
+ motor/pos_control module
+==========================
+:Author: Nicolas Schodet
+
+Introduction
+============
+
+This module provides position control, usually for a DC motor. Given a
+position consign, it will adjust output power so that the measured position
+matches the consign. There is no speed control, this is handled by another
+module.
+
+Usage
+=====
+
+The `pos_control_t` structure contains both current control state and control
+parameters. The `kp`, `kd` and `ki` parameters are the PID coefficients,
+using a fixed point representation. The `e_sat`, `i_sat` and `d_sat` are the
+maximum values for error, integral and differential values. They must be
+adapted so that PID computation does not overflow. See the
+`pos_control_compute_pid` function comments for guidelines to choose those
+values.
+
+The `cur` field is updated by this module with the current position. The
+`cons` field must be written by user to set the position consign. This module
+will try to make the current position matches the consign position.
+
+API
+===
+
+.. include:: pos_control.exd
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