From 048d8ad7e6549434a1a0877981874a99f8af7250 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 28 Mar 2004 20:53:57 +0000 Subject: Nouveau montage avec bit de sens. --- 2004/n/asserv/src/main.c | 19 +++++---- 2004/n/asserv/src/motor.c | 102 +++++++++++++++++++++++++++++----------------- 2 files changed, 74 insertions(+), 47 deletions(-) (limited to '2004/n/asserv') diff --git a/2004/n/asserv/src/main.c b/2004/n/asserv/src/main.c index e729ce4..d8f0622 100644 --- a/2004/n/asserv/src/main.c +++ b/2004/n/asserv/src/main.c @@ -54,9 +54,9 @@ A1-5 B2-7 C4-5 D2-6 E0-2 /* Configuration générale. */ #fuses H4,WDT,WDT128,PUT,NOBROWNOUT,NOLVP -#use delay(clock=39321600) +#use delay(clock=40000000) #use rs232(baud=9600,xmit=PIN_C6,parity=N,rcv=PIN_C7,bits=8) -#priority EXT,RDA,TBE,TIMER2 +#priority RB,RDA,TBE,TIMER2 #include "motor.c" #include "serial.c" @@ -69,18 +69,18 @@ main_init (void) setup_psp (PSP_DISABLED); /* Configuration de l'interface SPI : non utilisée. */ setup_spi (FALSE); - /* Le timer0 est utilisé pour les pas en avant du moteur droit. */ + /* Le timer0 est utilisé pour les pas du moteur gauche. */ setup_timer_0 (RTCC_EXT_L_TO_H | RTCC_8_BIT | RTCC_DIV_1); - /* Le timer 1 est utilisé pour les pas en arrière du moteur gauche. */ - setup_timer_1 (T1_EXTERNAL | T1_DIV_BY_1); + /* Le timer1 est inutilisé. */ + 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_4, 121, 10); - /* Le timer3 est utilisé pour les pas en arrière du moteur droit. */ + setup_timer_2 (T2_DIV_BY_4, 124, 10); + /* Le timer3 est utilisé pour les pas du moteur droit. */ setup_timer_3 (T3_EXTERNAL | T3_DIV_BY_1); /* Désactivation de l'ADC. */ setup_adc_ports (NO_ANALOGS); @@ -91,9 +91,10 @@ main_init (void) set_pwm1_duty (0); set_pwm2_duty (0); /* Configuration des interruptions activées. */ - enable_interrupts (INT_EXT); enable_interrupts (INT_TIMER2); enable_interrupts (INT_RDA); + disable_interrupts (INT_TBE); + enable_interrupts (INT_RB); enable_interrupts (GLOBAL); } @@ -101,8 +102,8 @@ void main (void) { delay_ms (20); + main_init (); motor_init (); serial_init (); - main_init (); motor_main (); } diff --git a/2004/n/asserv/src/motor.c b/2004/n/asserv/src/motor.c index f2ee2ab..d88fc8a 100644 --- a/2004/n/asserv/src/motor.c +++ b/2004/n/asserv/src/motor.c @@ -27,13 +27,13 @@ /* 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 +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 +unsigned int motor_pid_int; /* Compteur d'interruptions timer2 entre deux calculs de PID. */ /* Statistiques, etc... */ @@ -45,7 +45,7 @@ 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_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 @@ -53,6 +53,10 @@ signed long motor_g_e_old, motor_d_e_old; /* Derni 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) @@ -76,6 +80,8 @@ motor_init (void) 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; + motor_timer0_old = get_timer0 (); + motor_timer3_old = get_timer3 (); } /* Addition limitée à 32000. */ @@ -210,22 +216,15 @@ motor_compute_left_pid (void) signed long diff; signed long pwm; signed long temp; - unsigned char av, ar; - /* Récupère les compteurs. */ - av = motor_g_cpt_av; - 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; + /* Récupère les compteurs et calcule l'erreur. */ + disable_interrupts (INT_RB); + diff = motor_g_cpt_av; + motor_g_cpt_av = 0; + diff -= motor_g_cpt_ar; + motor_g_cpt_ar = 0; + enable_interrupts (INT_RB); 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); @@ -265,22 +264,15 @@ motor_compute_right_pid (void) 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; + /* Récupère les compteurs et calcule l'erreur. */ + disable_interrupts (INT_RB); + diff = motor_d_cpt_av; + motor_d_cpt_av = 0; + diff -= motor_d_cpt_ar; + motor_d_cpt_ar = 0; + disable_interrupts (INT_RB); 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); @@ -346,11 +338,40 @@ motor_d_pwm (signed long &pwm) } } -/* Interruptions de comptage moteur. */ -#int_EXT -EXT_isr() +/* 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; +} + +/* Interruptions de changement de sens. */ +#int_RB +RB_isr() { - motor_g_cpt_av++; + motor_update_cpt (); } /* Interruption de PID. */ @@ -373,6 +394,9 @@ motor_main (void) /* Calcul du PID. */ if ((motor_pid_int & 7) == 0) { + disable_interrupts (INT_RB); + motor_update_cpt (); + enable_interrupts (INT_RB); motor_compute_left_pid (); motor_compute_right_pid (); } @@ -449,9 +473,11 @@ motor_toggle_asservi (void) if (motor_asservi) { motor_g_cpt_av = 0; - motor_g_cpt_ar = get_timer1 (); - motor_d_cpt_av = get_timer0 (); - motor_d_cpt_ar = get_timer3 (); + 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; -- cgit v1.2.3