summaryrefslogtreecommitdiff
path: root/n/asserv/src/motor.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/motor.c')
-rw-r--r--n/asserv/src/motor.c402
1 files changed, 402 insertions, 0 deletions
diff --git a/n/asserv/src/motor.c b/n/asserv/src/motor.c
new file mode 100644
index 0000000..f54d989
--- /dev/null
+++ b/n/asserv/src/motor.c
@@ -0,0 +1,402 @@
+/* motor.c */
+/* APBTasserv - asservissement Robot 2005 {{{
+ *
+ * Copyright (C) 2004 Nicolas Schodet
+ *
+ * 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.
+ *
+ * Contact :
+ * Web: http://perso.efrei.fr/~schodet/
+ * Email: <contact@ni.fr.eu.org>
+ * }}} */
+#include "motor.h"
+#include "pwm.h"
+#include "timer.h"
+#include "counter.h"
+#include <n/avr/rs232/rs232.h>
+#include <n/avr/proto/proto.h>
+
+/* +AutoDec */
+/* -AutoDec */
+
+/* Variables globales. */
+uint8_t motor_asservi; /* Asservissement activé ? */
+uint8_t motor_pos_asserv; /* Si vrai, la carte d'asservissement est en
+ mode d'asservissement en position. C'est à
+ dire, l'ordinateur connecté à la carte
+ d'asservissement gère lui même la vitesse
+ afin d'asservir la position. En pratique,
+ - pas d'acquitement lors d'un !v
+ - arrét automatique si aucune commande !v
+ reçue aprés n cycles. Voir motor_ttl. */
+uint8_t motor_ttl; /* Time to live : arrète le robot si arrive à
+ 0 en mode asservissement position. */
+uint8_t motor_kp, motor_ki, motor_kd; /* Coefficients du PID. */
+uint8_t motor_a; /* Acceleration. */
+uint8_t motor_a_cpt; /* Compteur d'acceleration. */
+int16_t motor_int_max; /* Terme intégral maximum. */
+uint8_t motor_pid_int; /* Compteur d'interruptions timer entre deux
+ calculs de PID. */
+
+/* Statistiques, etc... */
+uint8_t motor_stat_delay; /* Delais entre les stats. */
+uint8_t motor_stat_delay_cpt; /* Compteur de stats. */
+uint8_t motor_cpt_delay; /* Delais entre rapport codeurs. */
+uint8_t motor_cpt_delay_cpt; /* Compteur entre les rapports. */
+
+/* Entrées. */
+uint8_t motor_gpi_delay; /* Delais entre deux envois. */
+uint8_t motor_gpi_delay_cpt; /* Compteur. */
+
+/* Moteurs. */
+int8_t motor_g_vdes, motor_g_vacc; /* Vitesse désirée, actuelle. */
+int8_t motor_d_vdes, motor_d_vacc;
+int16_t motor_g_cpt, motor_d_cpt; /* Compteurs. */
+int16_t motor_g_e_old, motor_d_e_old; /* Dernière erreur, pour le
+ calcul de la dérivée. */
+int16_t motor_g_pwm_old, motor_d_pwm_old; /* Dernière pwm, pour les
+ stats. */
+int16_t motor_g_int, motor_d_int; /* Valeur integrale. */
+int16_t motor_g_der, motor_d_der; /* Valeur dérivée. */
+
+/* On fait les cradingues... */
+#include "pwm.c"
+#include "timer.c"
+#include "counter.c"
+
+/* Initialise le module moteur. */
+void
+motor_init (void)
+{
+ motor_asservi = 0;
+ motor_pos_asserv = 0;
+ motor_ttl = 10;
+ motor_kp = 10; motor_ki = 5; motor_kd = 0;
+ motor_a = 8;
+ motor_a_cpt = 8;
+ motor_int_max = 1000;
+ motor_pid_int = 0;
+ motor_stat_delay = 0; motor_stat_delay_cpt = 0;
+ motor_cpt_delay = 0; motor_cpt_delay_cpt = 0;
+ motor_gpi_delay = 0; motor_gpi_delay_cpt = 0;
+ motor_g_vdes = 0; motor_g_vacc = 0;
+ motor_d_vdes = 0; motor_d_vacc = 0;
+ motor_g_cpt = 0; motor_d_cpt = 0;
+ motor_g_e_old = 0; motor_d_e_old = 0;
+ motor_g_pwm_old = 0; motor_d_pwm_old = 0;
+ motor_g_int = 0; motor_d_int = 0;
+ motor_g_der = 0; motor_d_der = 0;
+ pwm_init ();
+ timer_init ();
+ counter_init ();
+ rs232_init ();
+ proto_init (motor_serial_callback, rs232_putc);
+ proto_send0 ('z');
+}
+
+/* Limite un entier entre -MAX et MAX. */
+inline int16_t
+boundary (int16_t n, int16_t max)
+{
+ if (n > max)
+ return max;
+ if (n < -max)
+ return -max;
+ return n;
+}
+
+/* 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++;
+ }
+ else
+ {
+ /* Freine. */
+ motor_g_vacc--;
+ }
+ }
+}
+
+/* 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++;
+ }
+ else
+ {
+ /* Freine. */
+ motor_d_vacc--;
+ }
+ }
+}
+
+/* Calcule le PID associé au moteur gauche. */
+void
+motor_compute_left_pid (void)
+{
+ int16_t e;
+ int16_t diff;
+ int16_t pwm;
+ int16_t temp;
+ /* Récupère les compteurs et calcule l'erreur. */
+ diff = counter_l_frw;
+ counter_l_frw = 0;
+ diff -= counter_l_rev;
+ counter_l_rev = 0;
+ motor_g_cpt += diff;
+ e = motor_g_vacc - diff; /* 10b = 8b + 9b */
+ /* Calcul de l'integrale. */
+ motor_g_int = motor_g_int + e; /* 12b = 11b + 10b */
+ motor_g_int = boundary (motor_g_int, motor_int_max); /* 11b */
+ /* Calcul de la dérivée. */
+ //motor_g_der = safe_sub_long (e, motor_g_e_old);
+ /* Calcul du PID. P... */
+ pwm = e * motor_kp; /* 15b = 10b * 5b */
+ /* I... */
+ temp = motor_g_int * motor_ki; /* 15b = 11b * 4b */
+ pwm = pwm + temp; /* 16b = 15b + 15b */
+ /* D... */
+ //temp = safe_mul_long (motor_g_der, motor_kd);
+ //pwm = safe_add_long (pwm, temp);
+ /* Conserve l'ancienne erreur pour le terme dérivé. */
+ motor_g_e_old = e;
+ /* Conserve l'ancienne pwm. */
+ motor_g_pwm_old = pwm;
+}
+
+/* Calcule le PID associé au moteur droit. */
+void
+motor_compute_right_pid (void)
+{
+ int16_t e;
+ int16_t diff;
+ int16_t pwm;
+ int16_t temp;
+ /* Récupère les compteurs et calcule l'erreur. */
+ diff = counter_r_frw;
+ counter_r_frw = 0;
+ diff -= counter_r_rev;
+ counter_r_rev = 0;
+ motor_d_cpt += diff;
+ e = motor_d_vacc - diff;
+ /* Calcul de l'integrale. */
+ motor_d_int = motor_d_int + e;
+ motor_d_int = boundary (motor_d_int, motor_int_max);
+ /* Calcul de la dérivée. */
+ //motor_d_der = safe_sub_long (e, motor_d_e_old);
+ /* Calcul du PID. P... */
+ pwm = e * motor_kp;
+ /* I... */
+ temp = motor_d_int * motor_ki;
+ pwm = pwm + temp;
+ /* D... */
+ //temp = safe_mul_long (motor_d_der, motor_kd);
+ //pwm = safe_add_long (pwm, temp);
+ /* Conserve l'ancienne erreur pour le terme dérivé. */
+ motor_d_e_old = e;
+ /* Conserve l'ancienne pwm. */
+ motor_d_pwm_old = pwm;
+}
+
+/* Boucle principale. */
+void
+motor_main (void)
+{
+ while (1)
+ {
+ /* Ne fait le traitement que s'il y a eu une interruption. */
+ while (!timer_pending ())
+ counter_update ();
+ /* Calcul du PID. */
+ if (1)
+ {
+ motor_compute_left_pid ();
+ motor_compute_right_pid ();
+ /* Applique les nouvelles valeurs au même moment. */
+ if (motor_asservi)
+ {
+ pwm_set_left (motor_g_pwm_old);
+ pwm_set_right (motor_d_pwm_old);
+ }
+ else
+ {
+ pwm_set_left (0);
+ pwm_set_right (0);
+ }
+ /* Information de PWM. */
+ if (motor_stat_delay)
+ {
+ if (!--motor_stat_delay_cpt)
+ {
+ proto_send4 ('l', motor_g_vacc, motor_g_e_old,
+ motor_g_pwm_old >> 8, motor_g_pwm_old & 0xff);
+ proto_send4 ('r', motor_d_vacc, motor_d_e_old,
+ motor_d_pwm_old >> 8, motor_d_pwm_old & 0xff);
+ motor_stat_delay_cpt = motor_stat_delay;
+ }
+ }
+ /* Rapport des codeurs. */
+ if (motor_cpt_delay)
+ {
+ if (!--motor_cpt_delay_cpt)
+ {
+ proto_send4 ('C', motor_g_cpt >> 8, motor_g_cpt & 0xff,
+ motor_d_cpt >> 8, motor_d_cpt & 0xff);
+ motor_cpt_delay_cpt = motor_cpt_delay;
+ }
+ }
+ /* Accélère. */
+ if (motor_a)
+ {
+ if (!--motor_a_cpt)
+ {
+ motor_update_left_speed ();
+ motor_update_right_speed ();
+ motor_a_cpt = motor_a;
+ }
+ }
+ else
+ {
+ motor_g_vacc = motor_g_vdes;
+ motor_d_vacc = motor_d_vdes;
+ }
+ }
+ /* Le reste. */
+ if ((motor_pid_int & 7) == 0)
+ {
+ /* Gestion du ttl. */
+ if (motor_pos_asserv && (motor_g_vdes || motor_d_vdes) &&
+ --motor_ttl == 0)
+ {
+ motor_g_vdes = 0;
+ motor_d_vdes = 0;
+ //serial_send_motor_ttl ();
+ }
+ if (rs232_poll ())
+ proto_accept (rs232_getc ());
+ /* GPI. */
+ if (motor_gpi_delay)
+ {
+ if (!--motor_gpi_delay_cpt)
+ {
+ //serial_send_gpi (input_d ());
+ motor_gpi_delay_cpt = motor_gpi_delay;
+ }
+ }
+ }
+ motor_pid_int++;
+ }
+}
+
+/* Traite une entrée série. */
+void
+motor_serial_callback (uint8_t cmd, uint8_t argc, proto_arg_t argv[])
+{
+#define c(cmd, argc) (cmd << 8 | argc)
+ switch (c (cmd, argc))
+ {
+ case c ('z', 0):
+ reset ();
+ break;
+ case c ('v', 2):
+ motor_g_vdes = argv[0];
+ motor_d_vdes = argv[1];
+ if (motor_pos_asserv)
+ {
+ motor_ttl = 10;
+ return;
+ }
+ break;
+ case c ('V', 1):
+ motor_pos_asserv = argv[0];
+ break;
+ case c ('s', 0):
+ motor_g_vdes = 0;
+ motor_d_vdes = 0;
+ break;
+ case c ('a', 1):
+ motor_a = argv[0];
+ motor_a_cpt = motor_a;
+ break;
+ case c ('p', 1):
+ motor_kp = argv[0];
+ break;
+ case c ('i', 1):
+ motor_ki = argv[0];
+ break;
+ case c ('d', 1):
+ motor_kd = argv[0];
+ break;
+ case c ('m', 1):
+ motor_stat_delay = argv[0];
+ motor_stat_delay_cpt = motor_stat_delay;
+ break;
+ case c ('c', 1):
+ motor_cpt_delay = argv[0];
+ motor_cpt_delay_cpt = motor_cpt_delay;
+ break;
+ case c ('g', 1):
+ motor_asservi = argv[0];
+ motor_toggle_asservi ();
+ break;
+ case c ('h', 1):
+ motor_gpi_delay = argv[0];
+ motor_gpi_delay_cpt = motor_gpi_delay;
+ break;
+ case c ('k', 1):
+ //temp = argv[0];
+ //output_b (temp);
+ break;
+ case c ('D', 1):
+ //temp = argv[0];
+ //if (temp == 0x42)
+ //enable_interrupts (INT_EXT);
+ break;
+ default:
+ proto_send0 ('e');
+ return;
+ }
+ proto_send (cmd, argc, argv);
+#undef c
+}
+
+/* Démarre l'asservissement. */
+void
+motor_toggle_asservi (void)
+{
+ if (motor_asservi)
+ {
+ counter_reset ();
+ motor_g_vacc = 0;
+ motor_d_vdes = 0;
+ motor_g_vdes = 0;
+ motor_d_vdes = 0;
+ motor_g_int = 0;
+ motor_d_int = 0;
+ }
+}