/* 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. */ signed long motor_ttl; /* Time to live : arrète le robot au bout de ce nombre de pas. */ short motor_asservi; /* Asservissement activé ? */ unsigned int motor_kp, motor_ki, motor_kd; /* Coefficients du PID. */ unsigned int motor_a; /* Acceleration. */ signed long motor_int_max; /* Terme intégral maximum. */ unsigned int motor_pid_int; /* Compteur d'interruptions 2 entre deux calculs de PID. */ /* Statistiques, etc... */ short motor_stat; /* Report motor stats. */ 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; /* Compteur pour les interruptions. */ 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_int, motor_d_int; /* Valeur integrale. */ signed long motor_g_der, motor_d_der; /* Valeur dérivée. */ /* Initialise le module moteur. */ void motor_init (void) { motor_ttl = 2000; motor_asservi = 0; motor_kp = 10; motor_ki = 5; motor_kd = 0; motor_a = 2; motor_int_max = 1000; motor_pid_int = 0; motor_stat = 0; motor_stat_delay = 101; 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_int = 0; motor_d_int = 0; motor_g_der = 0; motor_d_der = 0; } /* 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 += motor_a; if (motor_g_vacc > motor_g_vdes) motor_g_vacc = motor_g_vdes; } else { /* Freine. */ motor_g_vacc -= motor_a; if (motor_g_vacc < motor_g_vdes) motor_g_vacc = motor_g_vdes; } } } /* 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 += motor_a; if (motor_d_vacc > motor_d_vdes) motor_d_vacc = motor_d_vdes; } else { /* Freine. */ motor_d_vacc -= motor_a; if (motor_d_vacc < motor_d_vdes) motor_d_vacc = motor_d_vdes; } } } /* 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; unsigned char av, ar; /* Récupère les compteurs. */ disable_interrupts (INT_EXT); av = motor_g_cpt_av; motor_g_cpt_av = 0; enable_interrupts (INT_EXT); disable_interrupts (INT_EXT1); ar = motor_g_cpt_ar; motor_g_cpt_ar = 0; enable_interrupts (INT_EXT1); /* Calcule l'erreur. */ diff = av; diff -= ar; 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); /* Information de PWM. */ if (motor_stat) { if (!motor_stat_delay_cpt--) { serial_send_motor_stat ('l', motor_g_vacc, e, pwm); motor_stat_delay_cpt = motor_stat_delay; } } /* Conserve l'ancienne erreur pour le terme dérivé. */ motor_g_e_old = e; } /* 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; unsigned char av, ar; /* Récupère les compteurs. */ av = get_timer0 (); ar = get_timer3 (); /* Calcule l'erreur. */ diff = av; diff -= motor_d_cpt_av; if (av < motor_d_cpt_av) diff += 256; diff -= ar; diff += motor_d_cpt_ar; if (ar < motor_d_cpt_ar) diff -= 256; motor_d_cpt += diff; e = motor_d_vacc - diff; /* Sauvegarde les compteurs. */ motor_d_cpt_av = av; motor_d_cpt_ar = ar; /* 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); /* Information de PWM. */ if (motor_stat) { if (!motor_stat_delay_cpt--) { serial_send_motor_stat ('r', motor_d_vacc, e, pwm); motor_stat_delay_cpt = motor_stat_delay; } } /* Conserve l'ancienne erreur pour le terme dérivé. */ motor_d_e_old = e; } /* 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); } } /* Interruptions de comptage moteur. */ #int_EXT EXT_isr() { motor_g_cpt_av++; } #int_EXT1 EXT1_isr() { motor_g_cpt_ar++; } /* Interruption de PID. */ #int_TIMER2 TIMER2_isr () { if ((motor_pid_int & 7) == 0) { motor_compute_left_pid (); motor_compute_right_pid (); } if ((motor_pid_int & 63) == 0) { motor_update_left_speed (); motor_update_right_speed (); 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); serial_send_ok ('v'); return 1; case 's': motor_g_vdes = 0; motor_d_vdes = 0; serial_send_ok ('s'); return 1; case 'a': serial_recv_int (motor_a); serial_send_ok ('a'); return 1; case 'p': serial_recv_int (motor_kp); serial_send_ok ('p'); return 1; case 'i': serial_recv_int (motor_ki); serial_send_ok ('i'); return 1; case 'd': serial_recv_int (motor_kd); serial_send_ok ('d'); return 1; case 'm': serial_recv_bool (motor_stat); serial_send_ok ('m'); return 1; case 'c': serial_recv_bool (motor_cpt); serial_send_ok ('c'); return 1; case 'g': serial_recv_bool (motor_asservi); motor_toggle_asservi (); serial_send_ok ('g'); 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 = get_timer0 (); motor_d_cpt_ar = 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; } }