From fe0e298c5b84e6b051fbc61fb826514f3da01c94 Mon Sep 17 00:00:00 2001 From: schodet Date: Thu, 11 Mar 2004 17:07:07 +0000 Subject: Passage à 40MHz. Utilisation du troisième compteur (non ! ils utilisent la même source !). Calcul du PID dans la boucle principale. Correction d'un bug dans le transfert serie (enable_interrupt trops tôt). --- 2004/n/asserv/src/main.c | 51 ++++++++++++++++------------------- 2004/n/asserv/src/motor.c | 67 +++++++++++++++++++++++++++------------------- 2004/n/asserv/src/motor.h | 4 +++ 2004/n/asserv/src/serial.c | 4 +-- 4 files changed, 69 insertions(+), 57 deletions(-) (limited to '2004/n') diff --git a/2004/n/asserv/src/main.c b/2004/n/asserv/src/main.c index 7f3732c..e729ce4 100644 --- a/2004/n/asserv/src/main.c +++ b/2004/n/asserv/src/main.c @@ -22,11 +22,10 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ -#include <18f442.h> // Définition des registres du PIC18 -#include // Définition de la librairie standard +#include <18f442.h> +#include -// Pattes libres. -/* +/* Pattes libres. 40 rb7 (coupee) 39 rb6 : D3 38 rb5 : D2 @@ -53,11 +52,11 @@ A1-5 B2-7 C4-5 D2-6 E0-2 */ -// General configuration -#fuses HS,WDT,WDT128,PUT,NOBROWNOUT,NOLVP -#use delay(clock=20000000) -#use rs232(baud=115200,xmit=PIN_C6,parity=N,rcv=PIN_C7,bits=8) -#priority EXT,EXT1,RDA,TBE,TIMER2 +/* Configuration générale. */ +#fuses H4,WDT,WDT128,PUT,NOBROWNOUT,NOLVP +#use delay(clock=39321600) +#use rs232(baud=9600,xmit=PIN_C6,parity=N,rcv=PIN_C7,bits=8) +#priority EXT,RDA,TBE,TIMER2 #include "motor.c" #include "serial.c" @@ -70,31 +69,29 @@ main_init (void) setup_psp (PSP_DISABLED); /* Configuration de l'interface SPI : non utilisée. */ setup_spi (FALSE); - // Configuration du chien de garde - //setup_wdt (WDT_OFF); - // Configuration du TIMER 0 pour lire le nombre de pas faits en reculant + /* Le timer0 est utilisé pour les pas en avant du moteur droit. */ setup_timer_0 (RTCC_EXT_L_TO_H | RTCC_8_BIT | RTCC_DIV_1); - // Confguration du TIMER 1 pour lire le nombre de pas faits en avançant - setup_timer_1 (T1_DISABLED); - // Configuration du TIMER 2. - // Tpwm = (PR2 + 1) * 4 * Tosc * TMR2_prescale - // Fpwm = 20kHz - // Tint2 = TMR2_postscale * Tpwm - // Tint2 = 0,5ms - setup_timer_2 (T2_DIV_BY_1, 249, 10); - // Configuration du TIMER 3 : non utilisé + /* Le timer 1 est utilisé pour les pas en arrière du moteur gauche. */ + setup_timer_1 (T1_EXTERNAL | T1_DIV_BY_1); + /* Configuration du TIMER 2. + * Tpwm = (PR2 + 1) * 4 * Tosc * TMR2_prescale + * Fpwm = 20kHz + * Tint2 = TMR2_postscale * Tpwm + * Tint2 = 0,5ms + */ + setup_timer_2 (T2_DIV_BY_4, 121, 10); + /* Le timer3 est utilisé pour les pas en arrière du moteur droit. */ setup_timer_3 (T3_EXTERNAL | T3_DIV_BY_1); - // Configuration de l'ADC + /* Désactivation de l'ADC. */ setup_adc_ports (NO_ANALOGS); setup_adc (ADC_CLOCK_DIV_2); - // Configuration des registres pour un fonctionnement en PWM + /* Configuration des registres pour un fonctionnement en PWM. */ setup_ccp1 (CCP_PWM); setup_ccp2 (CCP_PWM); set_pwm1_duty (0); set_pwm2_duty (0); - // Configuration pour l'autorisation des interruptions. + /* Configuration des interruptions activées. */ enable_interrupts (INT_EXT); - enable_interrupts (INT_EXT1); enable_interrupts (INT_TIMER2); enable_interrupts (INT_RDA); enable_interrupts (GLOBAL); @@ -107,7 +104,5 @@ main (void) motor_init (); serial_init (); main_init (); - while (1) - { - } + motor_main (); } diff --git a/2004/n/asserv/src/motor.c b/2004/n/asserv/src/motor.c index b529e87..f2ee2ab 100644 --- a/2004/n/asserv/src/motor.c +++ b/2004/n/asserv/src/motor.c @@ -26,6 +26,7 @@ #include "serial.h" /* Variables globales. */ +short motor_int_recv; /* Mis à 1 lors d'une interruption. */ signed long motor_ttl; /* Time to live : arrète le robot au bout de ce nombre de pas. */ short motor_asservi; /* Asservissement activé ? */ @@ -56,6 +57,7 @@ signed long motor_g_der, motor_d_der; /* Valeur d void motor_init (void) { + motor_int_recv = 0; motor_ttl = 2000; motor_asservi = 0; motor_kp = 10; motor_ki = 5; motor_kd = 0; @@ -210,19 +212,20 @@ motor_compute_left_pid (void) 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); + ar = get_timer1 (); /* Calcule l'erreur. */ diff = av; + diff -= motor_g_cpt_av; + if (av < motor_g_cpt_av) diff += 256; diff -= ar; + diff += motor_g_cpt_ar; + if (ar < motor_g_cpt_ar) diff -= 256; motor_g_cpt += diff; e = motor_g_vacc - diff; + /* Sauvegarde les compteurs. */ + motor_g_cpt_av = av; + motor_g_cpt_ar = ar; /* Calcul de l'integrale. */ motor_g_int = safe_add_long (motor_g_int, e); motor_g_int = boundary (motor_g_int, motor_int_max); @@ -350,33 +353,43 @@ 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_int_recv = 1; +} + +/* Boucle principale. */ +void +motor_main (void) +{ + while (1) { - motor_update_left_speed (); - motor_update_right_speed (); - serial_parse (); - if (motor_cpt) + /* Ne fait le traitement que s'il y a eu une interruption. */ + while (!motor_int_recv) + ; + motor_int_recv = 0; + /* Calcul du PID. */ + if ((motor_pid_int & 7) == 0) + { + motor_compute_left_pid (); + motor_compute_right_pid (); + } + /* Le reste. */ + if ((motor_pid_int & 63) == 0) { - serial_send_motor_cpt (motor_g_cpt, motor_d_cpt); - serial_unreliable_send_eob (); + 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++; } - motor_pid_int++; } /* Traite une entrée série. */ @@ -436,7 +449,7 @@ motor_toggle_asservi (void) if (motor_asservi) { motor_g_cpt_av = 0; - motor_g_cpt_ar = 0; + motor_g_cpt_ar = get_timer1 (); motor_d_cpt_av = get_timer0 (); motor_d_cpt_ar = get_timer3 (); motor_g_vacc = 0; diff --git a/2004/n/asserv/src/motor.h b/2004/n/asserv/src/motor.h index be29c03..d19bc4f 100644 --- a/2004/n/asserv/src/motor.h +++ b/2004/n/asserv/src/motor.h @@ -82,6 +82,10 @@ EXT1_isr(); #int_TIMER2 TIMER2_isr (); +/* Boucle principale. */ +void +motor_main (void); + /* Traite une entrée série. */ short motor_parse (void); diff --git a/2004/n/asserv/src/serial.c b/2004/n/asserv/src/serial.c index b8c18a3..d9776de 100644 --- a/2004/n/asserv/src/serial.c +++ b/2004/n/asserv/src/serial.c @@ -230,8 +230,8 @@ serial_send_char (char c) serial_send_full = 1; serial_send_buf_n = 0; c = serial_send_buf[serial_send_buf_n++]; - enable_interrupts (INT_TBE); putc (c); + enable_interrupts (INT_TBE); } } @@ -249,8 +249,8 @@ serial_unreliable_send_char (char c) serial_unreliable_send_full = 1; serial_unreliable_send_buf_n = 0; c = serial_unreliable_send_buf[serial_unreliable_send_buf_n++]; - enable_interrupts (INT_TBE); putc (c); + enable_interrupts (INT_TBE); } } -- cgit v1.2.3