From 7852fb953115ea9363d8e40dfa90fa869c257bf8 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 7 Dec 2003 13:02:17 +0000 Subject: Implémentation du terme D du PID. --- 2004/n/asserv/src/motor.c | 46 +++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 41 insertions(+), 5 deletions(-) (limited to '2004/n/asserv') diff --git a/2004/n/asserv/src/motor.c b/2004/n/asserv/src/motor.c index 94954f2..c820cc0 100644 --- a/2004/n/asserv/src/motor.c +++ b/2004/n/asserv/src/motor.c @@ -49,8 +49,9 @@ signed long motor_g_cpt, motor_d_cpt; /* Compteurs. */ signed long motor_g_e_old, motor_d_e_old; /* Dernière erreur, pour le calcul de la dérivée. */ signed long motor_g_int, motor_d_int; /* Valeur integrale. */ +signed long motor_g_der, motor_d_der; /* Valeur dérivée. */ -/* Initialise le modue moteur. */ +/* Initialise le module moteur. */ void motor_init (void) { @@ -70,6 +71,7 @@ motor_init (void) motor_g_cpt = 0; motor_d_cpt = 0; 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; } /* Addition limitée à 32000. */ @@ -88,6 +90,22 @@ safe_add_long (signed long &a, signed long &b) return ret; } +/* Soustraction limitée à 32000. */ +signed long +safe_sub_long (signed long &a, signed long &b) +{ + signed long ret; + /* Soustrait. */ + ret = a - b; + /* Vérifie les débordements par cohérence des signes. */ + if (a > 0 && b < 0 && ret <= 0) + return 32000; + else if (a < 0 && b > 0 && ret >= 0) + return -32000; + else + return ret; +} + /* Limite un entier entre -MAX et MAX. */ signed long boundary (signed long &n, signed long &max) @@ -206,10 +224,17 @@ motor_compute_left_pid (void) /* Calcul de l'integrale. */ motor_g_int = safe_add_long (motor_g_int, e); motor_g_int = boundary (motor_g_int, motor_int_max); - /* Calcul du PID. */ + /* Calcul de la dérivée. */ + motor_g_der = safe_sub_long (e, motor_g_e_old); + /* Calcul du PID. P... */ pwm = safe_mul_long (e, motor_kp); + /* I... */ temp = safe_mul_long (motor_g_int, motor_ki); pwm = safe_add_long (pwm, temp); + /* D... */ + temp = safe_mul_long (motor_g_der, motor_kd); + pwm = safe_add_long (pwm, temp); + /* Limite la valeur. */ temp = 1000; pwm = boundary (pwm, temp); /* Envois la sauce. */ @@ -224,7 +249,7 @@ motor_compute_left_pid (void) } } /* Conserve l'ancienne erreur pour le terme dérivé. */ - //motor_g_old_e = e; + motor_g_e_old = e; } /* Calcule le PID associé au moteur droit. */ @@ -254,10 +279,17 @@ motor_compute_right_pid (void) /* Calcul de l'integrale. */ motor_d_int = safe_add_long (motor_d_int, e); motor_d_int = boundary (motor_d_int, motor_int_max); - /* Calcul du PID. */ + /* Calcul de la dérivée. */ + motor_d_der = safe_sub_long (e, motor_d_e_old); + /* Calcul du PID. P... */ pwm = safe_mul_long (e, motor_kp); + /* I... */ temp = safe_mul_long (motor_d_int, motor_ki); pwm = safe_add_long (pwm, temp); + /* D... */ + temp = safe_mul_long (motor_d_der, motor_kd); + pwm = safe_add_long (pwm, temp); + /* Limite la valeur. */ temp = 1000; pwm = boundary (pwm, temp); /* Envois la sauce. */ @@ -272,7 +304,7 @@ motor_compute_right_pid (void) } } /* Conserve l'ancienne erreur pour le terme dérivé. */ - //motor_d_old_e = e; + motor_d_e_old = e; } /* Paramètre la PWM pour le moteur gauche. */ @@ -368,6 +400,10 @@ motor_parse (void) serial_recv_int (motor_ki); serial_send_ok ('i'); return 1; + case 'd': + serial_recv_int (motor_kd); + serial_send_ok ('d'); + return 1; case 'm': serial_recv_bool (motor_stat); serial_send_ok ('m'); -- cgit v1.2.3