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.c67
1 files changed, 40 insertions, 27 deletions
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érivée. */
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;