summaryrefslogtreecommitdiff
path: root/2004
diff options
context:
space:
mode:
Diffstat (limited to '2004')
-rw-r--r--2004/n/asserv/src/motor.c85
-rw-r--r--2004/n/asserv/src/serial.c30
-rw-r--r--2004/n/asserv/src/serial.h8
3 files changed, 81 insertions, 42 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;
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);