This is the third installment of my series describing the details of building your own self-balancing robot. In the first post, we covered the IMU to measure the tilt angle of the robot. The second post explained the foundations for controlling the robot to keep it balanced. In this post, we will finally get to the code to put it all together.
[Note: This series is intended to help you design and code your own self-balancing robot. I include lots of code in the articles, but I won’t have a download of all the code. Why? There are too many people who expect to download, compile and run code and have it work unchanged for their situation and then expect help from the author when it doesn’t work for them. This series will include everything you need and more. All of the code snippets can be copied and pasted and adjusted to your robot design – isn’t that what you’d do with a download anyway? The tips and explanations in these articles should hopefully help you get yours working.]
Program Design
In case you haven’t programmed an Arduino, here is a very brief overview. An Arduino program is called a Sketch. Your sketch is written in a programming language called Wiring which is based on Processing and looks like C++. In fact, you can and we will mix C++ into our robot code. You can develop your sketch in any IDE, but there is an Arduino-specific IDE that includes most of what you need. Additional libraries can be added to your project to extend the capabilities of the core framework. Arduino boards like the Arduino Uno we are using can be expanded through add-on boards called shields. The motor controller we will be using is an example. Your program is compiled on your desktop and transferred to your Arduino board through a serial link (typically a USB cable).
Your sketch has two functions: setup() and loop(). As the name implies, the setup() function is called once with the intention of getting things initialized and setup. The loop() function then runs continuously.
It’s About Time
[After writing that heading I was reminded of an old TV show that had an ear worm of a theme song. You’re welcome.]
One important aspect of an accurate IMU discussed in my first post, is the use of a consistent time interval. There are a number of hardware timers available on the Arduino boards that would provide just the thing we need for a consistent time interval. These timers allow your code to be called on a regular basis based on a hardware timer and an interrupt that calls your function. Since we are using an Uno board, there are three timers (timer0, timer1 and timer2) available two of which are 8 bits and one that is 16 bits. The number of bits determines the resolution of the timer and along with a scaling factor, how often your code is called. The typical Arduino board runs at 16MHz (there are some exceptions!) and the timers are incremented on every cycle or every 63 nanoseconds. When the value of the timer reaches a limit you specify, it wraps around to zero – this is called an overflow. The overflow creates an interrupt and calls your interrupt handler.
I elected to have the control loop run every 10 milliseconds or 100 times per second (a frequency of 100Hz). This was an arbitrary choice on my part and you could use something slower. If you think you need something faster, be sure to monitor how long your code takes to run within your loop.
Which timer should we use? Some libraries and functions use a specific timer, so be aware of the timers used by the libraries you use. For example, delay() and millis() use timer0 so I avoided using it. If you’re using the Servo library (which this project does not use) then you’ll want to avoid using timer1. How should we choose between the remaining two timers and how should they be configured? I found this detailed explanation very informative and I will summarize the concepts here.
You can specify how often the timer gets updated by specifying a divisor called the prescaler. The prescaler value can be one of 1, 8, 64, 256 or 1024. The Arduino will essentially divide the 16 MHz frequency by the prescaler to create a slower frequency. Instead of updating every 63 nanoseconds running full tilt, a value of 1024 would provide a counter frequency of 15.625 kHz or an update every 64 microseconds. Since we want a 10 millisecond interval, we would want the counter to count to 156.25 (10 milliseconds divided by 64 microseconds) or 156 ticks for an update cycle of 9.984 milliseconds. An 8 bit timer can hold a value of up to 255 so either an 8 bit or 16 bit timer would work. See the table below for the other options for a 100 Hz interval.
Now you could get into the low level register settings (and I may do that as an optimization later) but I decided to use the Timer1 Library for convenience. This library lets you specify the period of the update (e.g. 10 milliseconds) and the function to call when that time has expired.
Let’s Get The Party Started!
So let’s look at the code for setup().
Drivetrain _driveTrain;
IMU _imu;
State _state = STANDBY;
volatile bool _intervalPassed = false; // whether or not the PID feedback time interval has passed
unsigned long _lastTime = 0; // The last time the PID loop was evaluated
void setup() {
Serial.begin(115200);
if(!_driveTrain.start()){
Serial.println("Drivetrain failed");
standBy();
}
else {
Serial.println("Drivetrain OK");
}
if(!_imu.start()){
Serial.println("IMU failed");
standBy();
}
else {
Serial.println("IMU OK");
}
_lastTime = millis();
setupInterruptTimer();
}
The first you might notice is the use of a few global variables (starting with an underscore) for the drive train, IMU, state, interval passed and last time. The drive train and IMU are C++ abstractions for the motor controller shield and 9 DOF IMU breakout board. You might have noticed that there are is no call to new or malloc for the drive train or the IMU. This is one difference between Wiring and C++: Wiring allocates your C++ objects for you. The state variable is for the very simple state machine that toggles between RUNNING and STANDBY (an enum not shown here). When the robot tips over, we want the motors to shut down and wait until it is stood up again (STANDBY). When the robot is upright, it will start the motors again (RUNNING). The intervalPassed variable is a flag that indicates that our 10 millisecond interval has passed. Notice the volatile declaration? That’s a directive to the compiler not to use internal registers to cache the value for optimization because the loop() and the interruptHandler() both change the value and we want the most up to date value in memory when the loop() procedure reads it. Have a look at the relevant interrupt code below.
void setupInterruptTimer(){
Timer1.initialize(TIMER_INTERVAL_MICROS);
Timer1.attachInterrupt(timerInterruptHandler);
}
void timerInterruptHandler(){
_intervalPassed = true;
}
You can see that during setup(), the setupInterruptTimer() procedure is called. The Timer1 library is used to set up the interval (a static variable not shown here) and register the function to call when the interval is reached. The timerInterruptHandler() is called when the timer expires and all it does is set the intervalPassed global to true. The loop() sets this to false later. You always want to keep the code within a interrupt handler short and sweet.
Making it Go
This section covers a specific motor controller, namely the Adafruit Motor/Stepper/Servo Shield for Ardunio V2 kit V2.3. The class I used to encapsulate this choice can be used as a starting point for the motor controller that you are using. To start with, let’s have a look at the C++ header file snippet:
#include
#include
#define LEFT_MOTOR 4
#define RIGHT_MOTOR 1
class Drivetrain {
public:
Drivetrain(void);
bool start();
bool selfTest(uint8_t);
void forward(uint8_t);
void reverse(uint8_t);
void release();
void setSpeed(uint8_t);
private:
Adafruit_MotorShield _motorShield;
Adafruit_DCMotor *_leftMotor;
Adafruit_DCMotor *_rightMotor;
};
In retrospect, I probably should have named this Powertrain because it includes the motors but oh well. The interface consists of functions to initialize and control the direction and speed of the motors. The implementation of this C++ class looks like this:
#include "Arduino.h"
#include "Drivetrain.h"
Drivetrain::Drivetrain(){
_motorShield = Adafruit_MotorShield();
_leftMotor = _motorShield.getMotor(LEFT_MOTOR);
_rightMotor = _motorShield.getMotor(RIGHT_MOTOR);
}
bool Drivetrain::start(){
_motorShield.begin(); // using default PWM frequency
delay(1000); // wait for motor shield to start up
return true;
}
bool Drivetrain::selfTest(uint8_t aSpeed){
delay(500);
forward(aSpeed);
delay(500);
release();
reverse(aSpeed);
delay(500);
release();
return true;
}
void Drivetrain::forward(uint8_t aSpeed){
setSpeed(aSpeed);
_leftMotor->run(FORWARD);
_rightMotor->run(FORWARD);
}
void Drivetrain::reverse(uint8_t aSpeed){
setSpeed(aSpeed);
_leftMotor->run(BACKWARD);
_rightMotor->run(BACKWARD);
}
void Drivetrain::release(){
_leftMotor->run(RELEASE);
_rightMotor->run(RELEASE);
}
void Drivetrain::setSpeed(uint8_t aSpeed){
_leftMotor->setSpeed(aSpeed);
_rightMotor->setSpeed(aSpeed);
}
If you’re interested in the library used to interface with this shield have a look at this technical documentation.
Measuring the Tilt
This section covers a specific IMU breakout board, namely the Adafruit 9-DOF IMU Breakout – L3GD20H + LSM303. The class I used to encapsulate this choice can be used as a starting point for your IMU. To start with, let’s have a look at the C++ header file snippet:
#include
#include
#include
typedef enum {
APPROXIMATE = 0,
EXACT = 1
} IMU_Mode_t;
class IMU {
public:
IMU(void);
bool start();
bool selfTest(int iterations = 1000, int delayValue = 8);
float refresh(float);
bool reset();
void calibrate();
void measureARW();
void testFilter();
void findFilterTau();
void logData(bool);
private:
Adafruit_LSM303_Accel_Unified _accelerometer;
Adafruit_L3GD20_Unified _gyroscope;
float _angle = 0.0F;
IMU_Mode_t _calculationMode = EXACT;
float resetAccelerometer();
void calibrateGyroscope();
void calibrateAccelerometer();
bool _loggingData = false;
};
This article will cover only the essential functions leaving a few to be discussed when it comes time to tune our robot. One of the first things you’ll want to do is calibrate the accelerometer and gyroscope based on your implementation. I set my robot on a known level surface without the wheels and ran the calibration routines to establish the bias offsets:
float IMU_CALIBRATION_ANGLE = -0.09799358F; // DO NOT USE THESE VALUES FOR YOUR ROBOT
float IMU_CALIBRATION_OMEGA = -0.02030516F;
const int CALIBRATION_ITERATIONS = 100;
const int RESET_ITERATIONS = 50;
void IMU::calibrate(){
calibrateAccelerometer();
calibrateGyroscope();
}
void IMU::calibrateAccelerometer(){
Serial.print("Accel");
IMU_CALIBRATION_ANGLE = 0.0F;
float readings = 0.0F;
for(int i = 0; i < CALIBRATION_ITERATIONS; i++){
readings = readings + resetAccelerometer();
if(i % 5 == 0) {
Serial.print(".");
}
delay(100);
}
IMU_CALIBRATION_ANGLE = readings/((float)CALIBRATION_ITERATIONS);
Serial.println(" ");Serial.print("IMU_CALIBRATION_ANGLE = ");Serial.print(IMU_CALIBRATION_ANGLE, 8);Serial.println("F;");
}
float IMU::resetAccelerometer(){
sensors_event_t accelerometerEventData;
float reading = 0.0F;
for(int i = 0; i < RESET_ITERATIONS; i++){
_accelerometer.getEventGs(&accelerometerEventData);
float thisReading = constrain(accelerometerEventData.acceleration.z - IMU_CALIBRATION_ANGLE,-1.0F, 1.0F);
reading = reading + thisReading;
delay(10);
}
reading = reading/((float)RESET_ITERATIONS);
_angle = (-1.0F)*asin(reading);
return _angle;
}
void IMU::calibrateGyroscope(){
Serial.print("Gyro");
IMU_CALIBRATION_OMEGA = 0.0F;
sensors_event_t gyroEventData;
float readings = 0.0F;
for(int i = 0; i < CALIBRATION_ITERATIONS; i++){
_gyroscope.getEvent(&gyroEventData); // internally converts degrees/sec to radians/sec
readings = readings + gyroEventData.gyro.x;
if(i % 5 == 0) {
Serial.print(".");
}
delay(100);
}
IMU_CALIBRATION_OMEGA = readings/((float)CALIBRATION_ITERATIONS);
Serial.println(" ");Serial.print("IMU_CALIBRATION_OMEGA = ");Serial.print(IMU_CALIBRATION_OMEGA, 8);Serial.println("F;");
}
The calibration routines simply make the readings 100 times and averages them. Since the robot is not moving and is presumably level, any reported values are the offsets that need to be accounted for when the robot is moving. You may recall from my first article that the tilt angle equation is:
θ(t) = α * (θ(t – 1) + (ω – bias)* Δt) + (1 – α)*Az/g
where α is the high pass filter (HPF) coefficient for the gyroscope integration and (1-α) is the low pass filter (LPF) coefficient for the accelerometer-based small angle estimate. Here’s how this equation translates into code:
const float HPF = 0.94f; // Determined experimentally to reduce gyroscope drift. See Part 1
const float LPF = 1.0f - HPF;
float IMU::refresh(float dt){ // dt is in seconds
sensors_event_t accelerometerEventData;
sensors_event_t gyroEventData;
/*
Update the gyro and accel data from the device - the library gets all three dimensions
Accelerometer: Leaning forward reports a negative value - switch it around
Gyroscope: Leaning forward reports a positive value
*/
/*
The call below uses an updated version of the library to use native 'g' units
because sin(theta) = Az/g. The native result is in Gs, however
the Adafruit library multiplies this by g. We would ordinarily
end up dividing the result by g, so eliminate those steps by
keeping the units in Gs.
*/
_accelerometer.getEventGs(&accelerometerEventData);
_gyroscope.getEvent(&gyroEventData); // internally converts degrees/sec to radians/sec
float gyroRate = gyroEventData.gyro.x - IMU_CALIBRATION_OMEGA;
float gyroRotation = gyroRate * dt;
float accAngle = 0.0F;
float accReading = constrain(accelerometerEventData.acceleration.z + IMU_CALIBRATION_ANGLE,-1.0F, 1.0F); // contrained # of Gs
/*
With this hardware configuration, falling forward results in a negative acceleration due to gravity
and a positive angular velocity about the x-axis. Align the readings by multiplying the acceleration
by -1.0.
*/
if(_calculationMode == EXACT){
accAngle = (-1.0F)*asin(accReading);
}
else {
/* Approximate Mode
Because this value is a factor of g's, the value can be used
to approximate Sin(theta). At small angles (+/- 30 degrees),
this value can be used to closely estimate the angle of rotation (radians).
*/
accAngle = (-1.0F)*accReading;
}
/*
Use a Complementary filter to determine the angle of rotation by combining
the measurements of the accelerometer and gyroscope.
*/
_angle = HPF * (_angle + gyroRotation) + LPF * accAngle;
// Determine if a mode change is needed
if(_angle <= MAX_SMALL_ANGLE){
_calculationMode = APPROXIMATE;
}
else {
_calculationMode = EXACT;
}
return _angle;
}
Here is where you need to really understand what your accelerometer library returns. The Adafruit breakout board uses the ST Microelectronics LSM303DLHC. When polled, the accelerometer chip returns the number of G’s for each axis. The Adafruit library uses the accelerometer sensitivity settings (essentially how many thousandths of a G per bit in the A/D conversion) to arrive at the number of G’s. The library then multiplies this by 9.8 meters/sec² and returns the values. Returning values with units of meters/sec² makes sense for the library, but not for our use. The equation above (Az/g) would have us take that value and divide by 9.8 meters/sec² essentially undoing part of what the library just did. To avoid any rounding issues, I extended the Adafruit library to return the number G’s instead. You’ll also notice the IMU will switch between EXACT mode using the inverse sine (asin) function and APPROXIMATE mode when the angle is ±30 degrees.
Feeling Loopy
Now that we know how to measure the the tilt angle and make the robot move back and forth, it’s time to implement the loop() function. The Arduino framework automatically calls this function repeatably within a loop that runs forever. If you have a look at the design diagram, you’ll see that the function first checks to see if our interrupt driven interval has passed. If it has passed, then we read the angle from the IMU. The next set of decisions depends on the current angle reported by the IMU. If the robot is in STANDBY mode and the angle is now ±30 degrees, it switches over to RUNNING state. If it’s in RUNNING mode and the angle is outside this angle range, it shuts down the motors and switches to STANDBY. Once in RUNNING mode with a reported current angle of ±30 degrees, PID logic is invoked. Here is how the function looks so far:
const long MIN_THROTTLE = 50; // high enough to move the robot when tilted a few degrees
const long MAX_THROTTLE = 255;
// These values are determined experimentally - to be covered in the next article
const float Kp = 0.000160F;
const float Ki = 0.0060F;
const float Kd = 0.00000100F;
const float SCALER = 1000000.0f;
long MAX_CONTROL = (long)(Kp*SCALER*MAX_ANGLE_RADIANS_CONTROL + Ki*SCALER*TIMER_INTERVAL_SECONDS*MAX_ERROR_RADIANS + (Kd*SCALER*MAX_DELTA_ERROR_RADIANS/TIMER_INTERVAL_SECONDS));
void loop() {
unsigned long thisTime = millis();
if(!_intervalPassed){
// fall thru only if interval passed
return;
}
// Valid PID loop entry
float interval = (float)(thisTime - _lastTime)/1000.0F;
_lastTime = thisTime;
_currentError = _imu.refresh(interval); // current angle is the error
if(_state == STANDBY) {
if(abs(_currentError) <= MAX_ANGLE_RADIANS){ // Time to wake up Serial.print(thisTime);Serial.print(" OK: ");Serial.println(_currentError*RADIANS_TO_DEGREES, 5); reset(); } return; } else if(_state == RUNNING) { if(abs(_currentError) > MAX_ANGLE_RADIANS_CONTROL){
// There is no hope of balancing. Cut the motors
_driveTrain.release();
standBy();
return;
}
}
You may recall from my second article describing the PID controller, the equation for the controller feedback is:
The values of Kp, Ki and Kd appear to be “magic numbers” in the code above. There were determined experimentally and depend on many aspects of your robot so these values will not work for yours. The determination of these values will be covered in the next article as it’s one of the hardest parts of getting your robot to work. For now, we will go over how these values are used.
Before we go over the controller code, here is a map of the original equation to the associated code.
The actual code for the controller and throttle determination is shown below (a continuation of the loop() function).
if((_state == RUNNING) && (abs(_currentError) <= MAX_ANGLE_RADIANS)) { _intervalPassed = false; // reset for next time // PID control feedback _sumOfAllErrors = constrain(_sumOfAllErrors + _currentError, -MAX_ERROR_RADIANS, MAX_ERROR_RADIANS); float deltaError = _currentError - _previousError; long control = (long)((SCALER*Kp*_currentError) + (Ki*SCALER*interval*_sumOfAllErrors) + (Kd*SCALER*deltaError/interval)); long throttle = constrain(map(abs(control), 0L, MAX_CONTROL, MIN_THROTTLE, MAX_THROTTLE), MIN_THROTTLE, MAX_THROTTLE); if(control > 0){
_driveTrain.forward((uint8_t)throttle);
}
else{
if(control < 0) {
_driveTrain.reverse((uint8_t)throttle);
}
else {
_driveTrain.release();
}
}
_previousError = _currentError;
}
} // end of loop
void reset(){
if(_state == STANDBY){
_currentError = 0.0f;
_previousError = 0.0f;
_sumOfAllErrors = 0.0f;
delay(2500);
_lastTime = millis();
_imu.reset();
_state = RUNNING;
}
}
void standBy() {
if(_state == RUNNING){
delay(1000); // Let the IMU catch up to avoid false positives on the control angle envelope
Serial.println("** Waiting for near vertical **");
_currentError = 0.0f;
_previousError = 0.0f;
_sumOfAllErrors = 0.0f;
_state = STANDBY;
}
}
The calculation of the control variable includes a SCALER that is multiplied with each of the PID contributions. This is done to ensure that each contribution is large enough to make a difference across the spectrum of possible throttle values. This was found through experimentation and frustration during the tuning phase. The SCALER could be incorporated into the Kp, Ki and Kp values to save the math time. The sign of the control value determines the direction the robot needs to take. The absolute value of the control variable is mapped to a throttle range by using the map() function. This function does not guarantee that the result is within the supplied bounds so the constrain() function is used to ensure that the throttle is within the right range. The sign of the control variable and the resulting throttle is used to move the robot in the right direction with the right speed. In case the robot has achieved perfect balance, there is code to release the motors – probably only for a very short time.
Next we will tune the PID coefficients to make the robot balance itself.
Robotic Dreams 