Tabla de contenido
Publicidad
0xBC
stop PID
0xC1
M1 forward
0xC2
M1 backward
0xC5
M2 forward
0xC6
M2 backward
Código fuente
1.
#include <pololu/3pi.h>
2.
/*
3.
3pi-serial-slave - An example serial slave program for the Pololu
4.
3pi Robot.
5.
*/
6.
// PID constants
7.
unsigned int pid_enabled = 0;
8.
unsigned char max_speed = 255;
9.
unsigned char p_num = 0;
10.
unsigned char p_den = 0;
11.
unsigned char d_num = 0;
12.
unsigned char d_den = 0;
13.
unsigned int last_proportional = 0;
14.
unsigned int sensors[5];
15.
// This routine will be called repeatedly to keep the PID algorithm running
16.
void pid_check()
17.
{
18.
if(!pid_enabled)
19.
return;
20.
// Do nothing if the denominator of any constant is zero.
21.
if(p_den == 0 || d_den == 0)
22.
{
23.
set_motors(0,0);
24.
return;
25.
}
26.
// Read the line position, with serial interrupts running in the background.
27.
serial_set_mode(SERIAL_AUTOMATIC);
28.
unsigned int position = read_line(sensors, IR_EMITTERS_ON);
29.
serial_set_mode(SERIAL_CHECK);
30.
// The "proportional" term should be 0 when we are on the line.
31.
int proportional = ((int)position) - 2000;
32.
// Compute the derivative (change) of the position.
33.
int derivative = proportional - last_proportional;
34.
// Remember the last position.
35.
last_proportional = proportional;
36.
// Compute the difference between the two motor power settings,
37.
// m1 - m2.
38.
// to the right.
39.
// turn to the left, and the magnitude of the number determines
40.
// the sharpness of the turn.
41.
int power_difference = proportional*p_num/p_den + derivative*p_num/p_den;
42.
// Compute the actual motor settings.
43.
// to a negative value.
44.
if(power_difference > max_speed)
45.
power_difference = max_speed;
46.
if(power_difference < -max_speed)
47.
power_difference = -max_speed;
48.
if(power_difference < 0)
49.
set_motors(max_speed+power_difference, max_speed);
0
0
1
0
1
0
1
0
1
0
If this is a positive number the robot will turn
If it is a negative number, the robot will
siguientes a, b, c, d representan los parámetros, la
diferencia en la velocidad de los motores viene
dada por la expresión (L-2000)×a/b + D×c/d, en
donde L es la posición de la línea y D la derivada
de ésta L. La integral term no está en este
programa. Ver
Section 6.c
acerca del seguimiento de línea PID.
Para el seguimiento de línea PID motores a 0.
Motor M1 gira adelante a una velocidad de 0
(paro) hasta 127 (máximo avance).
Motor M1 gira atrás con una velocidad entre 0
(paro) hasta 127 (máximo retroceso).
Motor M2 gira adelante a una velocidad de 0
(paro) hasta 127 (máximo avance).
Motor M2 gira atrás con una velocidad entre 0
(paro) hasta 127 (máximo retroceso).
We never set either motor
para más información
Publicidad
Tabla de contenido
