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, 0 insertions, 402 deletions
diff --git a/n/asserv/src/motor.c b/n/asserv/src/motor.c
deleted file mode 100644
index f54d989..0000000
--- a/n/asserv/src/motor.c
+++ /dev/null
@@ -1,402 +0,0 @@
-/* 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;
- }
-}