7.1. Sensor measurements

The key to balancing is the built-in ST LSM6DS33 IMU chip, which combines a 3D gyroscope and a 3D accelerometer. The Balboa also includes an ST LIS3MDL 3-axis magnetometer. These nine sensor channels can be used in software to make an AHRS (attitude and heading reference system), a system that gives the robot a sense of its orientation in three dimensions. AHRS software is particularly important in aviation/drone applications, but for basic balancing, you don’t need anything that complicated. In fact, a single gyro channel – the y-axis – is enough to determine the robot’s angle of rotation relative to vertical.

The gyroscope’s y-axis channel measures the Balboa’s forward/backward rate of rotation, in units such as degrees per second. To keep track of its total rotation angle, you need to integrate the gyro reading: periodically check the sensor and multiply the rate of rotation by the time period. Here is the main gyro-related code from our Balboa Balancer example, which does exactly that:

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;
}

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 balancing, we know that on average, it must be close to vertical. So if we constantly, gradually shift the angle variable towards zero, it will not build up any significant error due to gyro drift. In our balancer example, that just takes a single line of code, called whenever the robot is balancing:

  angle = angle * 999 / 1000;

Now that we have good measurements of the robot’s angle and rate of rotation, we can talk about how to make it actually balance. The next section discusses the approach to balancing used in our example code.