summaryrefslogtreecommitdiffhomepage
path: root/digital/mimot/src/dirty/speed.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/mimot/src/dirty/speed.c')
-rw-r--r--digital/mimot/src/dirty/speed.c130
1 files changed, 130 insertions, 0 deletions
diff --git a/digital/mimot/src/dirty/speed.c b/digital/mimot/src/dirty/speed.c
new file mode 100644
index 00000000..f9d4193e
--- /dev/null
+++ b/digital/mimot/src/dirty/speed.c
@@ -0,0 +1,130 @@
+/* speed.c - Speed control. */
+/* asserv - Position & speed motor control on AVR. {{{
+ *
+ * Copyright (C) 2005 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.h"
+
+#include "modules/utils/utils.h"
+#include "modules/math/fixed/fixed.h"
+
+#include "pos.h"
+#include "state.h"
+
+/**
+ * This file is responsible for speed control. It changes the current shafts
+ * positions using ramps. It can be controlled by a wanted speed or wanted
+ * shaft position.
+ */
+
+/** Auxiliaries speed control states. */
+struct speed_t speed_aux[AC_ASSERV_AUX_NB];
+
+/** Initialise speed control states. */
+void
+speed_init (void)
+{
+ uint8_t i;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ speed_aux[i].pos = &pos_aux[i];
+}
+
+/** Update current speed according to a speed consign. */
+static void
+speed_update_by_speed (struct speed_t *speed)
+{
+ /* Update current speed. */
+ if (UTILS_ABS (speed->cons - speed->cur) < speed->acc)
+ speed->cur = speed->cons;
+ else if (speed->cons > speed->cur)
+ speed->cur += speed->acc;
+ else
+ speed->cur -= speed->acc;
+}
+
+/** Compute maximum allowed speed according to: distance left, maximum speed,
+ * current speed and acceleration. */
+static int16_t
+speed_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_update_by_position (struct speed_t *speed)
+{
+ int32_t diff = speed->pos_cons - speed->pos->cons;
+ speed->cur = speed_compute_max_speed (diff, speed->cur, speed->acc,
+ speed->max);
+}
+
+/** Update shaft position consign according to its consign type. */
+static void
+speed_update_by (struct speed_t *speed)
+{
+ if (speed->use_pos)
+ speed_update_by_position (speed);
+ else
+ speed_update_by_speed (speed);
+ /* Update shaft position. */
+ speed->pos->cons += speed->cur >> 8;
+}
+
+/** Update shaft position consign for one motor system. */
+static void
+speed_update_single (struct state_t *state, struct speed_t *speed)
+{
+ if (state->mode >= MODE_SPEED)
+ {
+ /* Update speed. */
+ speed_update_by (speed);
+ /* Check for completion. */
+ if (state->mode == MODE_SPEED
+ && speed->use_pos && speed->cur == 0)
+ state_finish (state);
+ }
+}
+
+/** Update shaft position consign according to consign. */
+void
+speed_update (void)
+{
+ uint8_t i;
+ for (i = 0; i < AC_ASSERV_AUX_NB; i++)
+ speed_update_single (&state_aux[i], &speed_aux[i]);
+}
+