summaryrefslogtreecommitdiff
path: root/2004/n/asserv/src/motor.c
diff options
context:
space:
mode:
authorschodet2003-09-30 22:17:01 +0000
committerschodet2003-09-30 22:17:01 +0000
commitea695ca13b725b6f2037986ef26fa1fea06b8faf (patch)
tree3dbe01ef7cca286eca1ad9433599699fa8f5ea52 /2004/n/asserv/src/motor.c
parent45e9ad53c377e594e03bf14fa4c6bab5322ada64 (diff)
Initial revision
Diffstat (limited to '2004/n/asserv/src/motor.c')
-rw-r--r--2004/n/asserv/src/motor.c402
1 files changed, 402 insertions, 0 deletions
diff --git a/2004/n/asserv/src/motor.c b/2004/n/asserv/src/motor.c
new file mode 100644
index 0000000..94954f2
--- /dev/null
+++ b/2004/n/asserv/src/motor.c
@@ -0,0 +1,402 @@
+/* motor.c */
+/* APBTasserv - asservissement Robot 2004 {{{
+ *
+ * Copyright (C) 2003 Nicolas Schodet
+ *
+ * Robot APB Team/Efrei 2004.
+ * Web: http://assos.efrei.fr/robot/
+ * Mail: 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.
+ *
+ * }}} */
+#include "motor.h"
+#include "serial.h"
+
+/* Variables globales. */
+signed long motor_ttl; /* Time to live : arrète le robot au bout de
+ ce nombre de pas. */
+short motor_asservi; /* Asservissement activé ? */
+unsigned int motor_kp, motor_ki, motor_kd; /* Coefficients du PID. */
+unsigned int motor_a; /* Acceleration. */
+signed long motor_int_max; /* Terme intégral maximum. */
+unsigned int motor_pid_int; /* Compteur d'interruptions 2 entre deux
+ calculs de PID. */
+
+/* Statistiques, etc... */
+short motor_stat; /* Report motor stats. */
+unsigned int motor_stat_delay; /* Delay between stats. */
+unsigned int motor_stat_delay_cpt; /* Delay counter. */
+
+/* Moteurs. */
+signed int motor_g_vdes, motor_g_vacc; /* Vitesse désirée, actuelle. */
+signed int motor_d_vdes, motor_d_vacc;
+unsigned int motor_g_cpt_av, motor_g_cpt_ar; /* Compteur pour les interruptions. */
+unsigned int motor_d_cpt_av, motor_d_cpt_ar;
+signed long motor_g_cpt, motor_d_cpt; /* Compteurs. */
+signed long motor_g_e_old, motor_d_e_old; /* Dernière erreur, pour le
+ calcul de la dérivée. */
+signed long motor_g_int, motor_d_int; /* Valeur integrale. */
+
+/* Initialise le modue moteur. */
+void
+motor_init (void)
+{
+ motor_ttl = 2000;
+ motor_asservi = 0;
+ motor_kp = 10; motor_ki = 5; motor_kd = 0;
+ motor_a = 2;
+ motor_int_max = 1000;
+ motor_pid_int = 0;
+ motor_stat = 0;
+ motor_stat_delay = 101;
+ motor_stat_delay_cpt = 0;
+ motor_g_vdes = 0; motor_g_vacc = 0;
+ motor_d_vdes = 0; motor_d_vacc = 0;
+ motor_g_cpt_av = 0; motor_g_cpt_ar = 0;
+ motor_d_cpt_av = 0; motor_d_cpt_ar = 0;
+ motor_g_cpt = 0; motor_d_cpt = 0;
+ motor_g_e_old = 0; motor_d_e_old = 0;
+ motor_g_int = 0; motor_d_int = 0;
+}
+
+/* Addition limitée à 32000. */
+signed long
+safe_add_long (signed long &a, signed long &b)
+{
+ signed long ret;
+ /* Additionne. */
+ ret = a + b;
+ /* Vérifie les débordements par cohérence des signes. */
+ if (a > 0 && b > 0 && ret <= 0)
+ return 32000;
+ else if (a < 0 && b < 0 && ret >= 0)
+ return -32000;
+ else
+ return ret;
+}
+
+/* Limite un entier entre -MAX et MAX. */
+signed long
+boundary (signed long &n, signed long &max)
+{
+ if (n > max)
+ return max;
+ if (n < -max)
+ return -max;
+ return n;
+}
+
+/* Multiplication par les coef. */
+signed long
+safe_mul_long (signed long &a, unsigned int &k)
+{
+ unsigned long ua;
+ short sa;
+ signed long temp;
+ /* Sépare le signe de la valeur absolue. */
+ if (a >= 0)
+ {
+ sa = 1;
+ ua = a;
+ }
+ else
+ {
+ sa = 0;
+ ua = -a;
+ }
+ /* Multiplie le poid fort. */
+ temp = make8 (ua, 1) * k;
+ /* Test un futur débordement. */
+ if (temp > 255)
+ {
+ if (sa) return 32760;
+ else return -32760;
+ }
+ else
+ {
+ temp = make8 (temp, 1);
+ temp += ((long) make8 (ua, 0)) * k ;
+ if (sa) return temp;
+ else return -temp;
+ }
+}
+
+/* Met à jour la vitesse du moteur gauche. */
+void
+motor_update_left_speed (void)
+{
+ if (motor_g_vacc != motor_g_vdes)
+ {
+ if (motor_g_vacc < motor_g_vdes)
+ {
+ /* Accélère. */
+ motor_g_vacc += motor_a;
+ if (motor_g_vacc > motor_g_vdes)
+ motor_g_vacc = motor_g_vdes;
+ }
+ else
+ {
+ /* Freine. */
+ motor_g_vacc -= motor_a;
+ if (motor_g_vacc < motor_g_vdes)
+ motor_g_vacc = motor_g_vdes;
+ }
+ }
+}
+
+/* Met à jour la vitesse du moteur droit. */
+void
+motor_update_right_speed (void)
+{
+ if (motor_d_vacc != motor_d_vdes)
+ {
+ if (motor_d_vacc < motor_d_vdes)
+ {
+ /* Accélère. */
+ motor_d_vacc += motor_a;
+ if (motor_d_vacc > motor_d_vdes)
+ motor_d_vacc = motor_d_vdes;
+ }
+ else
+ {
+ /* Freine. */
+ motor_d_vacc -= motor_a;
+ if (motor_d_vacc < motor_d_vdes)
+ motor_d_vacc = motor_d_vdes;
+ }
+ }
+}
+
+/* Calcule le PID associé au moteur gauche. */
+void
+motor_compute_left_pid (void)
+{
+ signed long e;
+ signed long diff;
+ signed long pwm;
+ signed long temp;
+ unsigned char av, ar;
+ /* Récupère les compteurs. */
+ disable_interrupts (INT_EXT);
+ av = motor_g_cpt_av;
+ motor_g_cpt_av = 0;
+ enable_interrupts (INT_EXT);
+ disable_interrupts (INT_EXT1);
+ ar = motor_g_cpt_ar;
+ motor_g_cpt_ar = 0;
+ enable_interrupts (INT_EXT1);
+ /* Calcule l'erreur. */
+ diff = av;
+ diff -= ar;
+ motor_g_cpt += diff;
+ e = motor_g_vacc - diff;
+ /* Calcul de l'integrale. */
+ motor_g_int = safe_add_long (motor_g_int, e);
+ motor_g_int = boundary (motor_g_int, motor_int_max);
+ /* Calcul du PID. */
+ pwm = safe_mul_long (e, motor_kp);
+ temp = safe_mul_long (motor_g_int, motor_ki);
+ pwm = safe_add_long (pwm, temp);
+ temp = 1000;
+ pwm = boundary (pwm, temp);
+ /* Envois la sauce. */
+ motor_g_pwm (pwm);
+ /* Information de PWM. */
+ if (motor_stat)
+ {
+ if (!motor_stat_delay_cpt--)
+ {
+ serial_send_motor_stat ('l', motor_g_vacc, e, pwm);
+ motor_stat_delay_cpt = motor_stat_delay;
+ }
+ }
+ /* Conserve l'ancienne erreur pour le terme dérivé. */
+ //motor_g_old_e = e;
+}
+
+/* Calcule le PID associé au moteur droit. */
+void
+motor_compute_right_pid (void)
+{
+ signed long e;
+ signed long diff;
+ signed long pwm;
+ signed long temp;
+ unsigned char av, ar;
+ /* Récupère les compteurs. */
+ av = get_timer0 ();
+ ar = get_timer3 ();
+ /* Calcule l'erreur. */
+ diff = av;
+ diff -= motor_d_cpt_av;
+ if (av < motor_d_cpt_av) diff += 256;
+ diff -= ar;
+ diff += motor_d_cpt_ar;
+ if (ar < motor_d_cpt_ar) diff -= 256;
+ motor_d_cpt += diff;
+ e = motor_d_vacc - diff;
+ /* Sauvegarde les compteurs. */
+ motor_d_cpt_av = av;
+ motor_d_cpt_ar = ar;
+ /* Calcul de l'integrale. */
+ motor_d_int = safe_add_long (motor_d_int, e);
+ motor_d_int = boundary (motor_d_int, motor_int_max);
+ /* Calcul du PID. */
+ pwm = safe_mul_long (e, motor_kp);
+ temp = safe_mul_long (motor_d_int, motor_ki);
+ pwm = safe_add_long (pwm, temp);
+ temp = 1000;
+ pwm = boundary (pwm, temp);
+ /* Envois la sauce. */
+ motor_d_pwm (pwm);
+ /* Information de PWM. */
+ if (motor_stat)
+ {
+ if (!motor_stat_delay_cpt--)
+ {
+ serial_send_motor_stat ('r', motor_d_vacc, e, pwm);
+ motor_stat_delay_cpt = motor_stat_delay;
+ }
+ }
+ /* Conserve l'ancienne erreur pour le terme dérivé. */
+ //motor_d_old_e = e;
+}
+
+/* Paramètre la PWM pour le moteur gauche. */
+void
+motor_g_pwm (signed long &pwm)
+{
+ if (!motor_asservi) pwm = 0;
+ if (pwm >= 0)
+ {
+ output_high (PIN_D1);
+ set_pwm2_duty(pwm);
+ }
+ else
+ {
+ output_low (PIN_D1);
+ set_pwm2_duty (-pwm);
+ }
+}
+
+/* Paramètre la PWM pour le moteur droit. */
+void
+motor_d_pwm (signed long &pwm)
+{
+ if (!motor_asservi) pwm = 0;
+ if (pwm >= 0)
+ {
+ output_high (PIN_D0);
+ set_pwm1_duty(pwm);
+ }
+ else
+ {
+ output_low (PIN_D0);
+ set_pwm1_duty (-pwm);
+ }
+}
+
+/* Interruptions de comptage moteur. */
+#int_EXT
+EXT_isr()
+{
+ motor_g_cpt_av++;
+}
+
+#int_EXT1
+EXT1_isr()
+{
+ motor_g_cpt_ar++;
+}
+
+/* Interruption de PID. */
+#int_TIMER2
+TIMER2_isr ()
+{
+ if ((motor_pid_int & 7) == 0)
+ {
+ motor_compute_left_pid ();
+ motor_compute_right_pid ();
+ }
+ if ((motor_pid_int & 63) == 0)
+ {
+ motor_update_left_speed ();
+ motor_update_right_speed ();
+ serial_parse ();
+ }
+ motor_pid_int++;
+}
+
+/* Traite une entrée série. */
+short
+motor_parse (void)
+{
+ switch (serial_recv_com ())
+ {
+ case 'v':
+ serial_recv_int (motor_g_vdes);
+ serial_recv_int1 (motor_d_vdes);
+ serial_send_ok ('v');
+ return 1;
+ case 's':
+ motor_g_vdes = 0;
+ motor_d_vdes = 0;
+ serial_send_ok ('s');
+ return 1;
+ case 'a':
+ serial_recv_int (motor_a);
+ serial_send_ok ('a');
+ return 1;
+ case 'p':
+ serial_recv_int (motor_kp);
+ serial_send_ok ('p');
+ return 1;
+ case 'i':
+ serial_recv_int (motor_ki);
+ serial_send_ok ('i');
+ return 1;
+ case 'm':
+ serial_recv_bool (motor_stat);
+ serial_send_ok ('m');
+ return 1;
+ case 'g':
+ serial_recv_bool (motor_asservi);
+ motor_toggle_asservi ();
+ serial_send_ok ('g');
+ return 1;
+ default:
+ return 0;
+ }
+}
+
+/* Démarre l'asservissement. */
+void
+motor_toggle_asservi (void)
+{
+ if (motor_asservi)
+ {
+ motor_g_cpt_av = 0;
+ motor_g_cpt_ar = 0;
+ motor_d_cpt_av = get_timer0 ();
+ motor_d_cpt_ar = get_timer3 ();
+ motor_g_vacc = 0;
+ motor_d_vdes = 0;
+ motor_g_vdes = 0;
+ motor_d_vdes = 0;
+ motor_g_int = 0;
+ motor_d_int = 0;
+ }
+}