summaryrefslogtreecommitdiff
path: root/digital/avr/modules/motor/control_system
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/control_system
parent7a1417eb3cecd3b5f5b8a5c1a73e5a89d19de2d4 (diff)
digital/avr/modules/motor: add motor control system
Diffstat (limited to 'digital/avr/modules/motor/control_system')
-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
5 files changed, 273 insertions, 0 deletions
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