summaryrefslogtreecommitdiff
path: root/n/asserv/src/speed.c
diff options
context:
space:
mode:
authorschodet2004-10-13 00:12:18 +0000
committerschodet2004-10-13 00:12:18 +0000
commit7868feb67d7c2d1414cbcd84b6f02a7305a65117 (patch)
tree35826ef1077ff133862b295f93242f89e4b7dc68 /n/asserv/src/speed.c
parentfdd6216054017f4aded7501d4a6e725710438a35 (diff)
Version toute nouvelle qu'elle est bien et bien organisée.
Gestion du mode pwm simple. Gestion de la virgule fixe dans les coefs. Préparation pour les améliorations futures. Séparation dans des fichiers différents. Netoyage.
Diffstat (limited to 'n/asserv/src/speed.c')
-rw-r--r--n/asserv/src/speed.c159
1 files changed, 159 insertions, 0 deletions
diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c
new file mode 100644
index 0000000..cb8f2db
--- /dev/null
+++ b/n/asserv/src/speed.c
@@ -0,0 +1,159 @@
+/* speed.c - PI speed control. */
+/* asserv - Position & speed motor control on a ATmega128. {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2005.
+ * Web: http://assos.efrei.fr/robot/
+ * Email: robot AT efrei DOT fr
+ *
+ * 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.
+ *
+ * }}} */
+
+/** Actual speed. */
+int8_t speed_left, speed_right;
+/** Wanted speed. */
+int8_t speed_left_aim, speed_right_aim;
+/** Acceleration value. */
+uint8_t speed_acc = 8;
+/** Acceleration counter, speed gets updated when it reachs 0. */
+uint8_t speed_acc_cpt;
+/** Integral term. */
+int16_t speed_left_int, speed_right_int;
+/** Integral max value. */
+int16_t speed_int_max = 1024;
+/** Last error value. */
+int16_t speed_left_e_old, speed_right_e_old;
+/** P coeficients. 5.8 fixed point format. */
+uint16_t speed_kp = 2 * 255;
+/** I coeficients. 4.8 fixed point format. */
+uint16_t speed_ki = 1 * 255;
+
+/* +AutoDec */
+
+/** Initialise speed parameters. */
+static inline void
+speed_init (void);
+
+/** Update speeds according to the wanted speeds and the acceleration. */
+static inline void
+speed_update (void);
+
+/** Compute new pwm value for left motor. */
+static inline void
+speed_compute_left_pwm (void);
+
+/** Compute new pwm value for right motor. */
+static inline void
+speed_compute_right_pwm (void);
+
+/** Forget past event, usefull when the speed control is disabled for some
+ * time. */
+static inline void
+speed_restart (void);
+
+/* -AutoDec */
+
+/** Initialise speed parameters. */
+static inline void
+speed_init (void)
+{
+ speed_acc = 8;
+ speed_int_max = 1024;
+ speed_kp = 2 * 255;
+ speed_ki = 1 * 255;
+}
+
+/** Update speeds according to the wanted speeds and the acceleration. */
+static inline void
+speed_update (void)
+{
+ if (speed_acc)
+ {
+ speed_acc_cpt--;
+ if (speed_acc_cpt == 0)
+ {
+ speed_acc_cpt = speed_acc;
+ /* Update speeds. */
+ if (speed_left > speed_left_aim)
+ speed_left--;
+ else if (speed_left < speed_left_aim)
+ speed_left++;
+ if (speed_right > speed_right_aim)
+ speed_right--;
+ else if (speed_right < speed_right_aim)
+ speed_right++;
+ }
+ }
+ else
+ {
+ speed_left = speed_left_aim;
+ speed_right = speed_right_aim;
+ }
+}
+
+/** Compute new pwm value for left motor. */
+static inline void
+speed_compute_left_pwm (void)
+{
+ int16_t e;
+ int16_t pwm;
+ e = speed_left - counter_left_diff; /* 10b = 8b + 9b */
+ /* Integral update. */
+ speed_left_int += e; /* 12b = 11b + 10b */
+ if (speed_left_int > speed_int_max) /* 11b */
+ speed_left_int = speed_int_max;
+ else if (speed_left_int < -speed_left_int)
+ speed_left_int = -speed_int_max;
+ /* Compute PI. */ /* 16b = 15b + 15b */
+ pwm = dsp_mul_i16f88 (e, speed_kp) /* 15b = 10b * 5.8b */
+ + dsp_mul_i16f88 (speed_left_int, speed_ki); /* 15b = 11b * 4.8b */
+ /* Save result. */
+ speed_left_e_old = e;
+ pwm_left = pwm;
+}
+
+/** Compute new pwm value for right motor. */
+static inline void
+speed_compute_right_pwm (void)
+{
+ int16_t e;
+ int16_t pwm;
+ e = speed_right - counter_right_diff;
+ /* Integral update. */
+ speed_right_int += e;
+ if (speed_right_int > speed_int_max)
+ speed_right_int = speed_int_max;
+ else if (speed_right_int < -speed_right_int)
+ speed_right_int = -speed_int_max;
+ /* Compute PI. */
+ pwm = dsp_mul_i16f88 (e, speed_kp)
+ + dsp_mul_i16f88 (speed_right_int, speed_ki);
+ /* Save result. */
+ speed_right_e_old = e;
+ pwm_right = pwm;
+}
+
+/** Forget past event, usefull when the speed control is disabled for some
+ * time. */
+static inline void
+speed_restart (void)
+{
+ speed_left_int = 0;
+ speed_right_int = 0;
+ speed_left = 0;
+ speed_right = 0;
+}