From 726812225fd35cbe508f2a23293f0781fd0d9d39 Mon Sep 17 00:00:00 2001 From: schodet Date: Sat, 17 Apr 2004 10:23:24 +0000 Subject: Gestion du ttl. Changement des acquitements. Mode asservissement en position (sans ack pour !v et ttl). Envois des motor stat. --- 2004/n/asserv/src/motor.c | 85 ++++++++++++++++++++++++++++++---------------- 2004/n/asserv/src/serial.c | 30 ++++++++++------ 2004/n/asserv/src/serial.h | 8 +++-- 3 files changed, 81 insertions(+), 42 deletions(-) (limited to '2004/n/asserv/src') 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; diff --git a/2004/n/asserv/src/serial.c b/2004/n/asserv/src/serial.c index d9776de..5b48cc9 100644 --- a/2004/n/asserv/src/serial.c +++ b/2004/n/asserv/src/serial.c @@ -45,7 +45,7 @@ int serial_send_buf_n; short serial_send_full; /* Tampon d'emmision non fiable. */ -#define SERIAL_UNRELIABLE_SEND_BUF_LEN 20 +#define SERIAL_UNRELIABLE_SEND_BUF_LEN 34 char serial_unreliable_send_buf[SERIAL_UNRELIABLE_SEND_BUF_LEN]; int serial_unreliable_send_buf_n; short serial_unreliable_send_full; @@ -130,7 +130,8 @@ serial_send () else putc (c); /* On ne fait pas le test, normalement ne doit pas arriver. - * assert (serial_send_buf_n < SERIAL_SEND_BUF_LEN); */ + * assert (serial_unreliable_send_buf_n < + * SERIAL_UNRELIABLE_SEND_BUF_LEN); */ } } } @@ -173,18 +174,18 @@ void serial_send_error (char c) { serial_send_char ('!'); - serial_send_char ('e'); + serial_send_char ('E'); serial_send_char (c); serial_send_char (CRLF); } -/* Envoie un code ok. */ +/* Acquitte la trame en la renvoyant à l'expediteur. */ void -serial_send_ok (char c) +serial_send_ok (void) { - serial_send_char ('!'); - serial_send_char ('o'); - serial_send_char (c); + int i; + for (i = 0; i < serial_recv_buf_n; i++) + serial_send_char (serial_recv_buf[i]); serial_send_char (CRLF); } @@ -193,7 +194,7 @@ void serial_send_motor_stat (char side, unsigned int vacc, unsigned long &e, unsigned long &pwm) { serial_unreliable_send_char ('!'); - serial_unreliable_send_char ('m'); + serial_unreliable_send_char ('M'); serial_unreliable_send_char (side); serial_unreliable_send_int (vacc); serial_unreliable_send_char (','); @@ -208,13 +209,22 @@ void serial_send_motor_cpt (signed long &g, signed long &d) { serial_unreliable_send_char ('!'); - serial_unreliable_send_char ('c'); + serial_unreliable_send_char ('C'); serial_unreliable_send_long (g); serial_unreliable_send_char (','); serial_unreliable_send_long (d); serial_unreliable_send_char (CRLF); } +/* Envoie un signal de fin de ttl. */ +void +serial_send_motor_ttl (void) +{ + serial_send_char ('!'); + serial_send_char ('T'); + serial_send_char (CRLF); +} + /* Envoie un caractère. */ void serial_send_char (char c) diff --git a/2004/n/asserv/src/serial.h b/2004/n/asserv/src/serial.h index 223d5bf..f371239 100644 --- a/2004/n/asserv/src/serial.h +++ b/2004/n/asserv/src/serial.h @@ -69,9 +69,9 @@ serial_parse (void); void serial_send_error (char c); -/* Envoie un code ok. */ +/* Acquitte la trame en la renvoyant à l'expediteur. */ void -serial_send_ok (char c); +serial_send_ok (void); /* Envoie un rapport d'un moteur. */ void @@ -81,6 +81,10 @@ serial_send_motor_stat (char side, unsigned int vacc, unsigned long &e, unsigned void serial_send_motor_cpt (signed long &g, signed long &d); +/* Envoie un signal de fin de ttl. */ +void +serial_send_motor_ttl (void); + /* Envoie un caractère. */ void serial_send_char (char c); -- cgit v1.2.3