User Guide

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 para más información
acerca del seguimiento de línea PID.
0xBC stop PID 0 0 Para el seguimiento de línea PID motores a 0.
0xC1 M1 forward 1 0 Motor M1 gira adelante a una velocidad de 0
(paro) hasta 127 (máximo avance).
0xC2 M1 backward 1 0 Motor M1 gira atrás con una velocidad entre 0
(paro) hasta 127 (máximo retroceso).
0xC5 M2 forward 1 0 Motor M2 gira adelante a una velocidad de 0
(paro) hasta 127 (máximo avance).
0xC6 M2 backward 1 0 Motor M2 gira atrás con una velocidad entre 0
(paro) hasta 127 (máximo retroceso).
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. If this is a positive number the robot will turn
38. // to the right. If it is a negative number, the robot will
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. We never set either motor
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);