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 --- .../avr/modules/motor/blocking_detection/Makefile | 5 + .../motor/blocking_detection/Makefile.module | 1 + .../motor/blocking_detection/blocking_detection.c | 54 ++++++++ .../motor/blocking_detection/blocking_detection.h | 53 ++++++++ .../blocking_detection/blocking_detection.txt | 21 +++ 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 +++++++ digital/avr/modules/motor/control_system/Makefile | 5 + .../modules/motor/control_system/Makefile.module | 1 + .../modules/motor/control_system/control_system.c | 142 +++++++++++++++++++ .../modules/motor/control_system/control_system.h | 92 +++++++++++++ .../motor/control_system/control_system.txt | 33 +++++ digital/avr/modules/motor/pos_control/Makefile | 5 + .../avr/modules/motor/pos_control/Makefile.module | 1 + .../avr/modules/motor/pos_control/pos_control.c | 103 ++++++++++++++ .../avr/modules/motor/pos_control/pos_control.h | 60 ++++++++ .../avr/modules/motor/pos_control/pos_control.txt | 32 +++++ 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 ++++++ 24 files changed, 1059 insertions(+) create mode 100644 digital/avr/modules/motor/blocking_detection/Makefile create mode 100644 digital/avr/modules/motor/blocking_detection/Makefile.module create mode 100644 digital/avr/modules/motor/blocking_detection/blocking_detection.c create mode 100644 digital/avr/modules/motor/blocking_detection/blocking_detection.h create mode 100644 digital/avr/modules/motor/blocking_detection/blocking_detection.txt 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 create mode 100644 digital/avr/modules/motor/control_system/Makefile create mode 100644 digital/avr/modules/motor/control_system/Makefile.module create mode 100644 digital/avr/modules/motor/control_system/control_system.c create mode 100644 digital/avr/modules/motor/control_system/control_system.h create mode 100644 digital/avr/modules/motor/control_system/control_system.txt create mode 100644 digital/avr/modules/motor/pos_control/Makefile create mode 100644 digital/avr/modules/motor/pos_control/Makefile.module create mode 100644 digital/avr/modules/motor/pos_control/pos_control.c create mode 100644 digital/avr/modules/motor/pos_control/pos_control.h create mode 100644 digital/avr/modules/motor/pos_control/pos_control.txt 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') 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 -- cgit v1.2.3