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 ++++++++++++++++++++++++++++++----------------- 1 file changed, 55 insertions(+), 30 deletions(-) (limited to '2004/n/asserv/src/motor.c') 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; -- cgit v1.2.3