The calibration variable
gYZero
, which is calculated during
setup()
, is an important feature.
Gyroscopes tend to have a small offset in their readings, which we measure in the example code by
taking readings while the robot is at rest. Correcting for the offset makes the angle calculation far more
accurate. Once calibrated, the angle should drift at most a few degrees in a minute, which is more
than good enough for balancing.
However, integrating the gyro reading actually only determines the total
change
in angle. You still need
to pick a good starting value. An easy way to do this is to start the robot in a known position; for
example you could initialize
angle
with a value of zero and start the robot from a vertical position.
Our balancer example expects the robot to start lying down on its front side, with the angle initialized
to 110 degrees.
Even after you have calibrated the gyro and estimated the starting angle, your angle measurement will
tend to drift over time, maybe a few degrees in a minute. In a full AHRS, you use the accelerometer
to eliminate drift, but for a balancing robot we can use a simpler trick: if the robot is successfully
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
void
setup()
{
// Initialize IMU.
Wire.begin();
if
(!imu.init())
{
while
(
true
)
{
Serial.println(
"Failed to detect and initialize IMU!"
);
delay(200);
}
}
imu.enableDefault();
imu.writeReg(LSM6::CTRL2_G, 0b01011000);
// 208 Hz, 1000 deg/s
// Wait for IMU readings to stabilize.
delay(1000);
// Calibrate the gyro.
int32_t total = 0;
for
(
int
i = 0; i < CALIBRATION_ITERATIONS; i++)
{
imu.read();
total += imu.g.y;
delay(1);
}
gYZero = total / CALIBRATION_ITERATIONS;
}
// Call this every 10ms (UPDATE_TIME_MS)
void
integrateGyro()
{
// Convert from full-scale 1000 deg/s to deg/s.
angleRate = (imu.g.y - gYZero) / 29;
angle += angleRate * UPDATE_TIME_MS;
}
Pololu Balboa 32U4 Balancing Robot User’s Guide
© 2001–2019 Pololu Corporation
7. How to make a Balboa balance
Page 79 of 97
