summaryrefslogtreecommitdiff
path: root/2004/n/asserv/src/motor.c
diff options
context:
space:
mode:
authorschodet2004-04-17 10:23:24 +0000
committerschodet2004-04-17 10:23:24 +0000
commit726812225fd35cbe508f2a23293f0781fd0d9d39 (patch)
tree851dbf7ca8978f06c55e8f0d88eccc04db90897d /2004/n/asserv/src/motor.c
parent2344ef4ae9f20240c63e53c5f8bd77d83f9e5bd3 (diff)
Gestion du ttl.
Changement des acquitements. Mode asservissement en position (sans ack pour !v et ttl). Envois des motor stat.
Diffstat (limited to '2004/n/asserv/src/motor.c')
-rw-r--r--2004/n/asserv/src/motor.c85
1 files changed, 55 insertions, 30 deletions
diff --git a/2004/n/asserv/src/motor.c b/2004/n/asserv/src/motor.c
index d88fc8a..095d132 100644
--- a/2004/n/asserv/src/motor.c
+++ b/2004/n/asserv/src/motor.c
@@ -27,9 +27,17 @@
/* 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é ? */
+short motor_pos_asserv; /* Si vrai, la carte d'asservissement est en
+ mode d'asservissement en position. C'est à
+ dire, l'ordinateur connecté à la carte
+ d'asservissement gère lui même la vitesse
+ afin d'asservir la position. En pratique,
+ - pas d'acquitement lors d'un !v
+ - arrét automatique si aucune commande !v
+ reçue aprés n cycles. Voir motor_ttl. */
+unsigned int motor_ttl; /* Time to live : arrète le robot si arrive à
+ 0 en mode asservissement position. */
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. */
@@ -50,6 +58,8 @@ 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
calcul de la dérivée. */
+signed long motor_g_pwm_old, motor_d_pwm_old; /* Dernière pwm, pour les
+ stats. */
signed long motor_g_int, motor_d_int; /* Valeur integrale. */
signed long motor_g_der, motor_d_der; /* Valeur dérivée. */
@@ -62,8 +72,9 @@ void
motor_init (void)
{
motor_int_recv = 0;
- motor_ttl = 2000;
motor_asservi = 0;
+ motor_pos_asserv = 0;
+ motor_ttl = 10;
motor_kp = 10; motor_ki = 5; motor_kd = 0;
motor_a = 2;
motor_int_max = 1000;
@@ -78,6 +89,7 @@ motor_init (void)
motor_d_cpt_av = 0; motor_d_cpt_ar = 0;
motor_g_cpt = 0; motor_d_cpt = 0;
motor_g_e_old = 0; motor_d_e_old = 0;
+ motor_g_pwm_old = 0; motor_d_pwm_old = 0;
motor_g_int = 0; motor_d_int = 0;
motor_g_der = 0; motor_d_der = 0;
motor_timer0_old = get_timer0 ();
@@ -243,17 +255,10 @@ motor_compute_left_pid (void)
pwm = boundary (pwm, temp);
/* Envois la sauce. */
motor_g_pwm (pwm);
- /* Information de PWM. */
- if (motor_stat)
- {
- if (!motor_stat_delay_cpt--)
- {
- serial_send_motor_stat ('l', motor_g_vacc, e, pwm);
- motor_stat_delay_cpt = motor_stat_delay;
- }
- }
/* Conserve l'ancienne erreur pour le terme dérivé. */
motor_g_e_old = e;
+ /* Conserve l'ancienne pwm pour les stats. */
+ motor_g_pwm_old = pwm;
}
/* Calcule le PID associé au moteur droit. */
@@ -291,17 +296,10 @@ motor_compute_right_pid (void)
pwm = boundary (pwm, temp);
/* Envois la sauce. */
motor_d_pwm (pwm);
- /* Information de PWM. */
- if (motor_stat)
- {
- if (!motor_stat_delay_cpt--)
- {
- serial_send_motor_stat ('r', motor_d_vacc, e, pwm);
- motor_stat_delay_cpt = motor_stat_delay;
- }
- }
/* Conserve l'ancienne erreur pour le terme dérivé. */
motor_d_e_old = e;
+ /* Conserve l'ancienne pwm pour les stats. */
+ motor_d_pwm_old = pwm;
}
/* Paramètre la PWM pour le moteur gauche. */
@@ -399,10 +397,30 @@ motor_main (void)
enable_interrupts (INT_RB);
motor_compute_left_pid ();
motor_compute_right_pid ();
+ /* Information de PWM. */
+ if (motor_stat)
+ {
+ if (!motor_stat_delay_cpt--)
+ {
+ serial_send_motor_stat ('l', motor_g_vacc, motor_g_e_old,
+ motor_g_pwm_old);
+ serial_send_motor_stat ('r', motor_d_vacc, motor_d_e_old,
+ motor_d_pwm_old);
+ serial_unreliable_send_eob ();
+ motor_stat_delay_cpt = motor_stat_delay;
+ }
+ }
}
/* Le reste. */
if ((motor_pid_int & 63) == 0)
{
+ /* Gestion du ttl. */
+ if (motor_pos_asserv && --motor_ttl == 0)
+ {
+ motor_g_vdes = 0;
+ motor_d_vdes = 0;
+ serial_send_motor_ttl ();
+ }
motor_update_left_speed ();
motor_update_right_speed ();
serial_parse ();
@@ -425,41 +443,48 @@ motor_parse (void)
case 'v':
serial_recv_int (motor_g_vdes);
serial_recv_int1 (motor_d_vdes);
- serial_send_ok ('v');
+ if (!motor_pos_asserv)
+ serial_send_ok ();
+ else
+ motor_ttl = 10;
+ return 1;
+ case 'V':
+ serial_recv_bool (motor_pos_asserv);
+ serial_send_ok ();
return 1;
case 's':
motor_g_vdes = 0;
motor_d_vdes = 0;
- serial_send_ok ('s');
+ serial_send_ok ();
return 1;
case 'a':
serial_recv_int (motor_a);
- serial_send_ok ('a');
+ serial_send_ok ();
return 1;
case 'p':
serial_recv_int (motor_kp);
- serial_send_ok ('p');
+ serial_send_ok ();
return 1;
case 'i':
serial_recv_int (motor_ki);
- serial_send_ok ('i');
+ serial_send_ok ();
return 1;
case 'd':
serial_recv_int (motor_kd);
- serial_send_ok ('d');
+ serial_send_ok ();
return 1;
case 'm':
serial_recv_bool (motor_stat);
- serial_send_ok ('m');
+ serial_send_ok ();
return 1;
case 'c':
serial_recv_bool (motor_cpt);
- serial_send_ok ('c');
+ serial_send_ok ();
return 1;
case 'g':
serial_recv_bool (motor_asservi);
motor_toggle_asservi ();
- serial_send_ok ('g');
+ serial_send_ok ();
return 1;
default:
return 0;