summaryrefslogtreecommitdiff
path: root/2004
diff options
context:
space:
mode:
authorschodet2004-03-28 20:53:57 +0000
committerschodet2004-03-28 20:53:57 +0000
commit048d8ad7e6549434a1a0877981874a99f8af7250 (patch)
tree7cfa5fc5f09826c16174a0d15abbb3151c48503c /2004
parent276febd690422bf29aeda900bc0eb8258a5d931e (diff)
Nouveau montage avec bit de sens.
Diffstat (limited to '2004')
-rw-r--r--2004/n/asserv/src/main.c19
-rw-r--r--2004/n/asserv/src/motor.c102
2 files changed, 74 insertions, 47 deletions
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ère erreur, pour le
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;