From 7868feb67d7c2d1414cbcd84b6f02a7305a65117 Mon Sep 17 00:00:00 2001 From: schodet Date: Wed, 13 Oct 2004 00:12:18 +0000 Subject: 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. --- n/asserv/src/motor.c | 402 --------------------------------------------------- 1 file changed, 402 deletions(-) delete mode 100644 n/asserv/src/motor.c (limited to 'n/asserv/src/motor.c') 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: - * }}} */ -#include "motor.h" -#include "pwm.h" -#include "timer.h" -#include "counter.h" -#include -#include - -/* +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; - } -} -- cgit v1.2.3