/* 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; } }