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