Pololu 0J5840, Руководство пользователя

Страница: 68 / 85

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
#include <pololu/3pi.h>
/*
* 3pi-serial-slave - An example serial slave program for the Pololu
* 3pi Robot.
See the following pages for more information:
*
*
http://www.pololu.com/docs/0J21
* http://www.pololu.com/docs/0J20
* http://www.poolu.com/
*
*/
// PID constants
unsigned int pid_enabled = 0;
unsigned char max_speed = 255;
unsigned char p_num = 0;
unsigned char p_den = 0;
unsigned char d_num = 0;
unsigned char d_den = 0;
unsigned int last_proportional = 0;
unsigned int sensors[5];
// This routine will be called repeatedly to keep the PID algorithm running
void pid_check()
{
if (!pid_enabled)
return ;
// Do nothing if the denominator of any constant is zero.
if (p_den == 0 || d_den == 0)
{
set_motors(0,0);
return ;
}
// Read the line position, with serial interrupts running in the background.
serial_set_mode(SERIAL_AUTOMATIC);
unsigned
int position = read_line(sensors, IR_EMITTERS_ON);
serial_set_mode(SERIAL_CHECK);
// The "proportional" term should be 0 when we are on the line.
int proportional = (( int )position) - 2000;
// Compute the derivative (change) of the position.
int derivative = proportional - last_proportional;
// Remember the last position.
last_proportional = proportional;
// Compute the difference between the two motor power settings,
// m1 - m2.
If this is a positive number the robot will turn
// to the right. If it is a negative number, the robot will
// turn to the left, and the magnitude of the number determines
// the sharpness of the turn.
int power_difference = proportional*p_num/p_den + derivative*p_num/p_den;
// Compute the actual motor settings. We never set either motor
// to a negative value.
if (power_difference > max_speed)
power_difference = max_speed;
if (power_difference < -max_speed)
power_difference = -max_speed;
?
Pololu 3pi Robot User’s Guide © 2001–2019 Pololu Corporation
10. Expansion Information Page 68 of 85