# Augmented Arthropod - Self-Balancing Mech  ## Step 7: PID Algorithm Implementation

Now I will introduce the more complex part of the code: the PID controller algorithm.

This type of algorithm is used in many automatic control applications. It can regulate all sorts of processes, from flow and temperature to leveling and speed. Essentially it is a closed feedback loop which takes in a variable as an input and produces an output in an attempt to drive the input to a specific set point.

PID stands for Proportional, Integral, and Derivative. Each of these terms affect the controllers response in different ways. Together they will produce an output which will drive our motors to keep our robot balanced.

• The proportional is the main driving term in the controller. It changes the controller output in proportion to the error (in our case the difference between the measured angle, and the desired angle). If the error becomes larger, then the gain from this term will increase proportionally.
• The integral term affects our robots response to the error based on the error's accumulation over time. If the error is large for a given period of time the increase/decrease will happen at a fast rate. Likewise if the error is small for a long period of time the changes will occur at a slower pace. You can think of this as the response based on how the system has behaved in the past.
• The derivative term produces an output based on the rate of change of the error. This translates to the difference between the current error and the previous error divided by the sampling period. This term will help in predicting how the balance of our robot will respond in the next reading. You can think of this term a predictive response on how the system will behave in the future.

So, now that we have a basic understanding of how the PID controller works in theory, let's go ahead and implement it into our class. We can start off by updating our header:

```#ifndef Monstro_h
#define Monstro_h

#include "Arduino.h"
#include <utility/imumaths.h>

class Monstro {

public:
Monstro(int leftForward, int leftBackward, int leftSpeedPin,
int rightForward, int rightBackward, int rightSpeedPin,
int trigA, int echoA, int trigB, int echoB);

// Behavior
bool Update();
void Initialize();
void ComputeBalance();

// IMU
volatile double xTilt;
volatile double yTilt;
volatile double zTilt;

private:

// IMU
void initializeIMU();

// Motors
int _leftForward;
int _leftBackward;
int _leftSpeedPin;
volatile int _leftSpeed = 0;
int _rightForward;
int _rightBackward;
int _rightSpeedPin ;
volatile int _rightSpeed = 0;
void initializeMotors();
void setMotors(int leftMotorSpeed, int rightMotorSpeed);

// PID
volatile float previous_error = 0, integral = 0;
volatile int motorPower;
float sampleTime = 0.005;
double outMin, outMax;
double _Kp, _Ki, _Kd;
volatile float Setpoint = 0, Input, Output;
void initializePID();
void SetTunings(double Kp, double Ki, double Kd);
void SetOutputLimits(double Min, double Max);

};
#endif```

There is a lot of new code here, and most of it may at first glance be difficult to comprehend, so I will go into detail:

• As you can see we have added another public function to our class: ComputeBalance(). This function will be called in a timer interrupt and consists of our PID controller's algorithm.
• We have also included a few variables which we will use in the actual implementation, like the previous_error, and the integral which we need to store between iterations.
• The motorPower is what we will use to drive our motors when calling the setMotors() function in the main Update() loop.
• The sampleTime is how often we will be calling the ComputeBalance function, in seconds.
• The variables outMin and outMax will help us constrain our output to values our motors are able to read (in our case these will be -255 to 255, however there may be cases where we want to change these.
• _Kp, _Ki, and _Kd are our proportional, integral, and derivative constants. These are what each part of the algorithm will be multiplied by.
• The Setpoint is our desired angle, and if our robot wants to stay balanced it should be set to 0. The input is what we will read from our IMU's tilt, and the Output is what the PID algorithm will give us.
• We also have an initializer function, and two more functions to help us tune the algorithm.

Now lets get into the source code implementation of the PID controller. This part is by no means finished, and we have written a few variations of this algorithm, but for the time being this version seems to work the best with our current setup:

```#include "Arduino.h"<br>#include "monstro.h"

Monstro::Monstro(int leftForward, int leftBackward, int leftSpeedPin,
int rightForward, int rightBackward, int rightSpeedPin,
int trigA, int echoA, int trigB, int echoB)
{
leftForward = leftForward;
_leftBackward = leftBackward;
_leftSpeedPin = leftSpeedPin;
_rightForward = rightForward;
_rightBackward = rightBackward;
_rightSpeedPin = rightSpeedPin;
}

// Behavior
void Monstro::Initialize() {
initializeIMU();
initializeMotors();
initializePID();
}
bool Monstro::Update() {
setMotors(motorPower, motorPower);
}

// IMU
void Monstro::initializeIMU() {
if (!_bno.begin())
{
Serial.print("No BNO055 detected");
while (1);
}
delay(1000);
_bno.setExtCrystalUse(true);
}
sensors_event_t event;
_bno.getEvent(&event);

xTilt = event.orientation.x;
yTilt = event.orientation.y;
zTilt = event.orientation.z;
}

// Motors
void Monstro::initializeMotors() {
pinMode(_leftForward, OUTPUT);
pinMode(_leftBackward, OUTPUT);
pinMode(_leftSpeedPin, OUTPUT);
pinMode(_rightForward, OUTPUT);
pinMode(_rightBackward, OUTPUT);
pinMode(_rightSpeedPin, OUTPUT);
}
void Monstro::setMotors(int leftMotorSpeed, int rightMotorSpeed) {
if (rightMotorSpeed <= 0) {
digitalWrite(_rightBackward, LOW);
digitalWrite(_rightForward, HIGH);
analogWrite(_rightSpeedPin, abs(rightMotorSpeed));
}
else {
digitalWrite(_rightBackward, HIGH);
digitalWrite(_rightForward, LOW);
analogWrite(_rightSpeedPin, rightMotorSpeed);
}
if (leftMotorSpeed <= 0) {
digitalWrite(_leftBackward, LOW);
digitalWrite(_leftForward, HIGH);
analogWrite(_leftSpeedPin, abs(leftMotorSpeed));
}
else {
digitalWrite(_leftBackward, HIGH);
digitalWrite(_leftForward, LOW);
analogWrite(_leftSpeedPin, leftMotorSpeed);
}
}

// PID
void Monstro::initializePID() {
SetOutputLimits(-250, 250);
SetTunings(25, 0.5, 275);
}
void Monstro::ComputeBalance() {
Input = zTilt;

// Compute error variables
float error = Input - Setpoint;

// Calculate proportional component
float proportional = error * _Kp;

// Calculate integral component
integral += error * _Ki;
integral = constrain(integral, outMin, outMax); // limit wind-up

// Calculate derivative component
float derivative = (error - previous_error) * _Kd;

// Save variables for next error computation
previous_error = error;

// Add up PID
Output = proportional + integral + derivative;
// Limit to PWM constraints
Output = constrain(Output, outMin, outMax);

// Motor control
motorPower = Output;

// give up if there is no chance of success
if (Input < -40 || Input > 40) motorPower = 0;
}
void Monstro::SetTunings(double Kp, double Ki, double Kd) {
_Kp = Kp;
_Ki = Ki;
_Kd = Kd;
}
void Monstro::SetOutputLimits(double Min, double Max) {
if (Min > Max) return;
outMin = Min;
outMax = Max;
}```

Let's go over our changes:

• We included the setMotors() function inside of the Update() loop. This will ensure that each time the main loop runs our motors' rotation and speed is updated according to the output provided by the PID controller (motorPower).
• The initializePID() function is added to our master Initialize() function. It is used to set our constants, and set our minimum and maximum outputs.
• The ComputeBalance() function itself is where the PID calculations happen. It starts by taking in the zTilt of our robot as the input. Then we compute the error by checking on the difference between the input and our setpoint (which should be 0 if we are keeping the robot balanced). We then compute the proportional term by multiplying it against the error. Followed by adding the error*integral constant to our integral term, and constraining it to our maximum and minimum in order to limit the wind up this term can cause (in case our robot falls down for a while and we want to pick it back up without it acting crazy). The derivative is then calculated by checking on the difference between our current error and our last measured error, and multiplying that by our derivative constant. We then save our current error as our last error, and add up our terms together to compute our Output. This Output can now be assigned to the motorPower which will be read to drive our motors in the main Update() loop. Last but not least we also want to make sure to turn the motorPower to 0 in case our robot tilt is beyond a point where it can recover from, so that when it falls down it doesnt keep spinning its wheels and destroys itself.