summaryrefslogtreecommitdiff
path: root/2004/n/asserv/src/motor.c
diff options
context:
space:
mode:
Diffstat (limited to '2004/n/asserv/src/motor.c')
-rw-r--r--2004/n/asserv/src/motor.c44
1 files changed, 22 insertions, 22 deletions
diff --git a/2004/n/asserv/src/motor.c b/2004/n/asserv/src/motor.c
index 095d132..2f05c3a 100644
--- a/2004/n/asserv/src/motor.c
+++ b/2004/n/asserv/src/motor.c
@@ -40,12 +40,12 @@ 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. */
+unsigned int motor_a_cpt; /* Compteur d'acceleration. */
signed long motor_int_max; /* Terme intégral maximum. */
unsigned int motor_pid_int; /* Compteur d'interruptions timer2 entre deux
calculs de PID. */
/* Statistiques, etc... */
-short motor_stat; /* Report motor stats. */
unsigned int motor_stat_delay; /* Delay between stats. */
unsigned int motor_stat_delay_cpt; /* Delay counter. */
short motor_cpt; /* Report motors counters. */
@@ -76,11 +76,11 @@ motor_init (void)
motor_pos_asserv = 0;
motor_ttl = 10;
motor_kp = 10; motor_ki = 5; motor_kd = 0;
- motor_a = 2;
+ motor_a = 8;
+ motor_a_cpt = 8;
motor_int_max = 1000;
motor_pid_int = 0;
- motor_stat = 0;
- motor_stat_delay = 101;
+ motor_stat_delay = 0;
motor_stat_delay_cpt = 0;
motor_cpt = 0;
motor_g_vdes = 0; motor_g_vacc = 0;
@@ -183,16 +183,12 @@ motor_update_left_speed (void)
if (motor_g_vacc < motor_g_vdes)
{
/* Accélère. */
- motor_g_vacc += motor_a;
- if (motor_g_vacc > motor_g_vdes)
- motor_g_vacc = motor_g_vdes;
+ motor_g_vacc++;
}
else
{
/* Freine. */
- motor_g_vacc -= motor_a;
- if (motor_g_vacc < motor_g_vdes)
- motor_g_vacc = motor_g_vdes;
+ motor_g_vacc--;
}
}
}
@@ -206,16 +202,12 @@ motor_update_right_speed (void)
if (motor_d_vacc < motor_d_vdes)
{
/* Accélère. */
- motor_d_vacc += motor_a;
- if (motor_d_vacc > motor_d_vdes)
- motor_d_vacc = motor_d_vdes;
+ motor_d_vacc++;
}
else
{
/* Freine. */
- motor_d_vacc -= motor_a;
- if (motor_d_vacc < motor_d_vdes)
- motor_d_vacc = motor_d_vdes;
+ motor_d_vacc--;
}
}
}
@@ -398,9 +390,9 @@ motor_main (void)
motor_compute_left_pid ();
motor_compute_right_pid ();
/* Information de PWM. */
- if (motor_stat)
+ if (motor_stat_delay)
{
- if (!motor_stat_delay_cpt--)
+ if (!--motor_stat_delay_cpt)
{
serial_send_motor_stat ('l', motor_g_vacc, motor_g_e_old,
motor_g_pwm_old);
@@ -410,19 +402,25 @@ motor_main (void)
motor_stat_delay_cpt = motor_stat_delay;
}
}
+ /* Accélère. */
+ if (!--motor_a_cpt)
+ {
+ motor_update_left_speed ();
+ motor_update_right_speed ();
+ motor_a_cpt = motor_a;
+ }
}
/* Le reste. */
if ((motor_pid_int & 63) == 0)
{
/* Gestion du ttl. */
- if (motor_pos_asserv && --motor_ttl == 0)
+ if (motor_pos_asserv && (motor_g_vdes || motor_d_vdes) &&
+ --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 ();
if (motor_cpt)
{
@@ -459,6 +457,7 @@ motor_parse (void)
return 1;
case 'a':
serial_recv_int (motor_a);
+ motor_a_cpt = motor_a;
serial_send_ok ();
return 1;
case 'p':
@@ -474,7 +473,8 @@ motor_parse (void)
serial_send_ok ();
return 1;
case 'm':
- serial_recv_bool (motor_stat);
+ serial_recv_int (motor_stat_delay);
+ motor_stat_delay_cpt = motor_stat_delay;
serial_send_ok ();
return 1;
case 'c':