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/control_state/Makefile | 5 + .../modules/motor/control_state/Makefile.module | 1 + .../modules/motor/control_state/control_state.h | 111 +++++++++++++++++++++ .../modules/motor/control_state/control_state.txt | 47 +++++++++ 4 files changed, 164 insertions(+) create mode 100644 digital/avr/modules/motor/control_state/Makefile create mode 100644 digital/avr/modules/motor/control_state/Makefile.module create mode 100644 digital/avr/modules/motor/control_state/control_state.h create mode 100644 digital/avr/modules/motor/control_state/control_state.txt (limited to 'digital/avr/modules/motor/control_state') 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 -- cgit v1.2.3