/* 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. */ short motor_int_recv; /* Mis à 1 lors d'une interruption. */ short motor_asservi; /* Asservissement activé ? */ short 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. */ unsigned int motor_ttl; /* Time to live : arrète le robot si arrive à 0 en mode asservissement position. */ unsigned int motor_kp, motor_ki, motor_kd; /* Coefficients du PID. */ unsigned int motor_a; /* Acceleration. */ unsigned int motor_a_cpt; /* Compteur d'acceleration. */ signed long motor_int_max; /* Terme intégral maximum. */ unsigned int motor_pid_int; /* Compteur d'interruptions timer2 entre deux calculs de PID. */ /* Statistiques, etc... */ unsigned int motor_stat_delay; /* Delay between stats. */ unsigned int motor_stat_delay_cpt; /* Delay counter. */ short motor_cpt; /* Report motors counters. */ /* 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; /* Compteurs avant/arrière. */ 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_pwm_old, motor_d_pwm_old; /* Dernière pwm, pour les stats. */ signed long motor_g_int, motor_d_int; /* Valeur integrale. */ signed long motor_g_der, motor_d_der; /* Valeur dérivée. */ /* Comptage hard. */ unsigned int motor_timer0_old, motor_timer3_old; /* Ancienne valeur du compteur hard. */ /* Initialise le module moteur. */ void motor_init (void) { motor_int_recv = 0; 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 = 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_pwm_old = 0; motor_d_pwm_old = 0; motor_g_int = 0; motor_d_int = 0; motor_g_der = 0; motor_d_der = 0; motor_timer0_old = get_timer0 (); motor_timer3_old = get_timer3 (); } /* 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; } /* Soustraction limitée à 32000. */ signed long safe_sub_long (signed long &a, signed long &b) { signed long ret; /* Soustrait. */ 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++; } 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) { signed long e; signed long diff; signed long pwm; signed long temp; /* Récupère les compteurs et calcule l'erreur. */ diff = motor_g_cpt_av; motor_g_cpt_av = 0; diff -= motor_g_cpt_ar; motor_g_cpt_ar = 0; 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 de la dérivée. */ motor_g_der = safe_sub_long (e, motor_g_e_old); /* Calcul du PID. P... */ pwm = safe_mul_long (e, motor_kp); /* I... */ temp = safe_mul_long (motor_g_int, motor_ki); pwm = safe_add_long (pwm, temp); /* D... */ temp = safe_mul_long (motor_g_der, motor_kd); pwm = safe_add_long (pwm, temp); /* Limite la valeur. */ temp = 1000; pwm = boundary (pwm, temp); /* Envois la sauce. */ motor_g_pwm (pwm); /* Conserve l'ancienne erreur pour le terme dérivé. */ motor_g_e_old = e; /* Conserve l'ancienne pwm pour les stats. */ motor_g_pwm_old = pwm; } /* 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; /* Récupère les compteurs et calcule l'erreur. */ diff = motor_d_cpt_av; motor_d_cpt_av = 0; diff -= motor_d_cpt_ar; motor_d_cpt_ar = 0; motor_d_cpt += diff; e = motor_d_vacc - diff; /* 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 de la dérivée. */ motor_d_der = safe_sub_long (e, motor_d_e_old); /* Calcul du PID. P... */ pwm = safe_mul_long (e, motor_kp); /* I... */ temp = safe_mul_long (motor_d_int, motor_ki); pwm = safe_add_long (pwm, temp); /* D... */ temp = safe_mul_long (motor_d_der, motor_kd); pwm = safe_add_long (pwm, temp); /* Limite la valeur. */ temp = 1000; pwm = boundary (pwm, temp); /* Envois la sauce. */ motor_d_pwm (pwm); /* Conserve l'ancienne erreur pour le terme dérivé. */ motor_d_e_old = e; /* Conserve l'ancienne pwm pour les stats. */ motor_d_pwm_old = pwm; } /* 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); } } /* Met à jour les compteurs. */ inline void motor_update_cpt (void) { unsigned int timer; /* Récupère le compteur gauche. */ timer = get_timer0 (); if (input_b () & 0x10) { motor_g_cpt_ar += timer - motor_timer0_old; } else { motor_g_cpt_av += timer - motor_timer0_old; } motor_timer0_old = timer; /* Récupère le compteur droit. */ timer = get_timer3 (); if (input_b () & 0x80) { motor_d_cpt_av += timer - motor_timer3_old; } else { motor_d_cpt_ar += timer - motor_timer3_old; } motor_timer3_old = timer; } /* Interruption de PID. */ #int_TIMER2 TIMER2_isr () { motor_int_recv = 1; } /* Boucle principale. */ void motor_main (void) { while (1) { /* Ne fait le traitement que s'il y a eu une interruption. */ while (!motor_int_recv) motor_update_cpt (); motor_int_recv = 0; /* Calcul du PID. */ if ((motor_pid_int & 7) == 0) { motor_compute_left_pid (); motor_compute_right_pid (); /* Information de PWM. */ if (motor_stat_delay) { if (!--motor_stat_delay_cpt) { serial_send_motor_stat ('l', motor_g_vacc, motor_g_e_old, motor_g_pwm_old); serial_send_motor_stat ('r', motor_d_vacc, motor_d_e_old, motor_d_pwm_old); serial_unreliable_send_eob (); motor_stat_delay_cpt = motor_stat_delay; } } /* Accélère. */ if (!--motor_a_cpt) { motor_update_left_speed (); motor_update_right_speed (); motor_a_cpt = motor_a; } } /* Le reste. */ if ((motor_pid_int & 63) == 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 (); } serial_parse (); if (motor_cpt) { serial_send_motor_cpt (motor_g_cpt, motor_d_cpt); serial_unreliable_send_eob (); } } 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); if (!motor_pos_asserv) serial_send_ok (); else motor_ttl = 10; return 1; case 'V': serial_recv_bool (motor_pos_asserv); serial_send_ok (); return 1; case 's': motor_g_vdes = 0; motor_d_vdes = 0; serial_send_ok (); return 1; case 'a': serial_recv_int (motor_a); motor_a_cpt = motor_a; serial_send_ok (); return 1; case 'p': serial_recv_int (motor_kp); serial_send_ok (); return 1; case 'i': serial_recv_int (motor_ki); serial_send_ok (); return 1; case 'd': serial_recv_int (motor_kd); serial_send_ok (); return 1; case 'm': serial_recv_int (motor_stat_delay); motor_stat_delay_cpt = motor_stat_delay; serial_send_ok (); return 1; case 'c': serial_recv_bool (motor_cpt); serial_send_ok (); return 1; case 'g': serial_recv_bool (motor_asservi); motor_toggle_asservi (); serial_send_ok (); 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 = 0; motor_d_cpt_ar = 0; motor_timer0_old = get_timer0 (); motor_timer3_old = 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; } }