summaryrefslogtreecommitdiff
path: root/2004/n/asserv/src/motor.c
diff options
context:
space:
mode:
Diffstat (limited to '2004/n/asserv/src/motor.c')
-rw-r--r--2004/n/asserv/src/motor.c102
1 files changed, 64 insertions, 38 deletions
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;