Self-Balancing Robot using PSOC™ 6 & Arduino

Don’t we all wish for one article that will help us find our balance? While this might not be the article you’re looking for to find balance in life, it is the article to help your robot find its balance! We are taking you on a journey to learn how to use a PID controller to tune your robot’s motion so it can maintain its balance. We will be using PSOC™ 6, Infineon's TLE94112 DC motor control shield and some motors. Follow along and by the end you will be able to create your own self-balancing robot. Let’s get started!

Crash Course: PID Control:

The main aim of this project is to maintain the robot’s balance on two wheels. In order to achieve this, a form of feedback control over the robot’s current state is needed to continuously provide corrective actions so the robot can find its balance. This can be achieved by integrating a simple PID controller into our system. If you are unfamiliar with this controller, you’re in the right place. Let me give you a quick briefing about it so we can get on with our project!

PID controller stands for Proportional, Integral and Derivative controller, and is used to control a certain process variable, such as speed or torque of a given system. This happens by taking sensor readings as input and calculating how far we are from the desired or reference value, i.e. the error. The next step would be to apply the controller parameters, known as the P, I and D gains, to the error value in order to correct it via a new control signal. This is achieved through multiplying the error by each of the controller gains by using specific techniques for each term's calculation and adding the three values to make up the control signal.

The proportional term P directly multiplies the error by the proportional gain Kp. The integral term I applies an integration of the error over the time period and multiplies this result by the integral gain Ki. Integration over time fundamentally means summing the current error to all the previous errors. The derivative term D applies a differentiation of the error over the time period and multiplies this result by the derivative gain Kd. Applying differentiation on the error fundamentally means finding the change in the error over a time step.

This process continues as a feedback loop until the system reaches its target state. A visual representation of this control technique is seen in the following block diagram:

In our case, the target state is a balanced robot, which is judged through sensory readings of the robot’s angle. These sensor readings are the feedback values continuously sent to the controller. The control signal, i.e. the corrective action, is a PWM signal translating to a new motor speed. A designer’s role, in other words YOUR role, is to tune the three control parameters to get the most effective actions that help the system reach its target state.

System Overview:

The basic concept of a wheeled self-balancing robot is to get the angle by which the robot is tilting and whether it is a forward or backward tilt. If the angle is forward, the robot should move forward in order to regain its balance. The speed with which the wheels should move in the designated direction depends on the amount of tilt angle, if the angle is small, the wheels should move at a slower speed. To get this information we will use the BMI270 6-axis sensor that is located on the PSOC™ 6 AI kit. The sensor will give us gyroscope and accelerometer readings for the robot's current rotation angles and accelerations.

In order to get the most relevant data out of these readings, we will use a complimentary filter that assigns a weight factor, Alpha, to both readings. This is how we will observe our system to decide what is the best course of action.

Our next step will be to generate a control signal to the motors via the PID controller to determine their motion. Once the controller has calculated the motor speeds, in terms of PWM signal, the PSOC™ 6 kit will interface with the Multi Half Bridge Motor Shield TLE94112. In case you need a refresher on how Half Bridges work, we highly recommend checking out our PSOC™ 6 & Arduino with Multi Half-Bridge Protip.

Here is a system overview diagram to summarize everything we have mentioned so far

Now that you have a full grasp of what we need to do, let's get started on the implementation!

Hardware Setup:

To tackle the hardware set up, you will need all the components linked above, as well as some 3D printed pieces that make up your robot.

3D Printed Parts:

To build your robot from scratch, you will need to 3D print parts to make up the robot and the electronic components’ housing. The robot is divided into two levels, each with its own functionality. All the levels will be glued together in the end to form our robot.

Let’s start with the lower part of the robot, where we will fix the motors and the battery. As you can see below, we created some holes in the bottom part to fix the robot wheel motors. Moreover, we created a compartment to fix the battery within. These features will help the robot become more balanced.

Next, we move on to the top part where we will fix the Motor shield and the PSOC™ 6 kit. Our design includes openings for screws that will fix the motor shield onto the base, while the PSOC™ 6 kit can be easily placed into the housing compartment.

The complete assembly of the robot will be as follows

So, let’s get printing!

Connections:

Software:

This project utilizes the power of Infineon's PSOC™ 6 AI kit as the brain of our system. To get started with PSOC™ 6, make sure you downloaded the PSOC™ 6 for Arduino. If you need support with this, refer to our Protip: PSOC™ 6 for Arduino.

The PSOC™ 6 AI kit needs to know how to interface with its onboard BMI270 sensor and the TLE94112 multi-half-bridge motor control board. For this, you will need to include some libraries in your Arduino IDE.

#include <tle94112-ino.hpp> //Include TLE94112 Motor Shield Library
#include <Arduino_BMI270_BMM150.h> // Include Arduino BMI270 library

For the BMI-270, download the.ZIP file attached to this article then follow these steps: In your Arduino IDE, navigate to Sketch > IncludeLibrary > Add.ZIP Library and select the ZIP file you downloaded.

For the TLE94112, navigate to the Arduino library manager and search for multi-half-bridge. Install the latest version.

Initialization

Step 1: Create the required variables for your PID controller. You will need the constants for the proportional, integral and derivative gains. Additionally, you need to define some intermediate variables to keep track of your system state during operation:

// PID constants
float Kp = 10;
float Ki = 0.02;
float Kd = 0.03;
// Variables for PID control
float setPoint = 0.0; // Desired angle
float currentAngle = 0.0;
float errorSum = 0;
float lastError= 0;
float previous_error = 0.0; //Previous error for derivative calculation
float integral = 0.0;
float dt = 0.01;

Step 2: Define the variables for the complimentary filter to the BMI270 Sensor.

// Complimentary filter variables
float accAngle, gyroRate, angle;
float alpha = 0.99; // Filter Coefficient
unsigned long lastTime = 0;

Step 3: Define the registers used for motor control and create the motor object.

//! Tle94112 registers for motor 1 and 2
uint8_t motorReg[2][2] = {
{ REG_ACT_1, REG_PWM_DC_1 },
{ REG_ACT_3, REG_PWM_DC_3 }
};

//! Tle94112 register for motor direction
volatile uint8_t oldDirection[] = { LL_HH, HH_LL };

// Tle94112 object for motor controller
Tle94112Ino controller = Tle94112Ino(3, 4);

Setup

Moving on to the Setup() function, initialize the serial port, with Baudrate of 9600 and initialize the motor controller.

void setup() {
// Initialize serial communication
Serial.begin(9600);

// Initialize the motor controller
controller.begin();

Next, configure the Multi-Half Bridge and PWM channels for the motors.

// Configure half bridges and PWM channels for motors
controller.configHB(controller.TLE_HB1, controller.TLE_HIGH, controller.TLE_PWM1);
controller.configHB(controller.TLE_HB2, controller.TLE_HIGH, controller.TLE_PWM1);
controller.configHB(controller.TLE_HB3, controller.TLE_LOW, controller.TLE_NOPWM);
controller.configHB(controller.TLE_HB4, controller.TLE_LOW, controller.TLE_NOPWM);
controller.configHB(controller.TLE_HB9, controller.TLE_HIGH, controller.TLE_PWM3);
controller.configHB(controller.TLE_HB10, controller.TLE_HIGH, controller.TLE_PWM3);
controller.configHB(controller.TLE_HB11, controller.TLE_LOW, controller.TLE_NOPWM);
controller.configHB(controller.TLE_HB12, controller.TLE_LOW, controller.TLE_NOPWM);
// All stop
controller.configPWM(controller.TLE_PWM1, controller.TLE_FREQ200HZ, 0);
controller.configPWM(controller.TLE_PWM3, controller.TLE_FREQ200HZ, 0);
controller.clearErrors();

Finally, initialize the BMI sensor.

// Initialize BMI270
if (!IMU.begin()) {
Serial.println("Failed to initialize BMI270!");
while (1);    // Halt execution if IMU initialization fails
}

Functions

Function 1: motorSet.

It first checks the direction the motor is running in and if it's changed it calls the directWriteReg function to update the direction through the registers we defined above. This means it changes the Multi Half Bridge operation from Low Side Switch to High Side Switch and vice versa. The function takes as inputs the motor number, direction, speed and an error check variable.

void motorSet(uint8_t motorNum, uint8_t dir, uint8_t speed, bool errorCheck = false) {

if (dir != oldDirection[motorNum]) {
controller.directWriteReg(motorReg[motorNum][0], dir);
oldDirection[motorNum] = dir;
}

Secondly, it sets the duty cycle using the directWriteReg function. The duty cycle ranges from 0-255. Finally, we verify the errorCheck input variable. This variable checks for any errors in the controller object, if none it clears errors in the controller object. Otherwise, it will be detected by the function in the next run.

controller.directWriteReg(motorReg[motorNum][1], speed);
if (errorCheck) {
controller.clearErrors();
}
}

Function 2: motor_pwm.

This function takes as input the motor number and its speed. Based on the sign of the speed it determines the direction of the motor. It will then call the motorSet function.

void motor_pwm(int motor, int speed) {
if (speed > 0) {
motorSet(motor, LL_HH, abs(speed));
} else {
motorSet(motor, HH_LL, abs(speed));
}
}

If you need a refresher on the concepts PWM, go to our protip: Using PWM with Micropython on the PSoC. Note that the coding part in the protip is using MicroPython while we are using Arduino code in this article.

Function 3: motor_ramp.

This function acts as a safety net for the motor operation we are using. If the motor duty cycle is below 20, this will correspond to a voltage that is too low to make the motor turn. However, we must check first if the input duty cycle is zero, meaning we don't want the motor to turn. In that case we will exit the function.

int16_t motor_ramp(int16_t motor_in) {
if (abs(motor_in) < 1) {
return 0;
}
if (motor_in > 0) {
return motor_in + 20;
} else {
return motor_in - 20;
}
}

Loop

This part is the actual operation of our code, that will run continuously, making use of all the pieces we have defined so far. We start the operation by checking if the sensor data is available.

void loop() {
if (IMU.accelerationAvailable()) {

Within this if statement, all our components will come together.

Step 1:Find the angle after applying the filter.

We begin by reading accelerometer and gyroscope data.

Using the readAcceleration function, we save accelerometer data into three float variables ax, ay and az. Using the readGyroscope function, we repeat the same process to read and save the gyroscope data.

if (IMU.accelerationAvailable()) {
float ax, ay, az;
IMU.readAcceleration(ax, ay, az);  // Read acceleration values
float gx, gy, gz;
IMU.readGyroscope(gx, gy, gz);

From there we can calculate the tilt angle using a simple mathematical formula and save it into the variable accAngle.

// Calculate the current angle (e.g., pitch) based on acceleration values
accAngle = ax / az;
accAngle = atan(accAngle) * 180 / PI;  // Convert to degrees
gyroRate = gy;

Finally, apply the complimentary filter to get the filtered angle value.

We need to calculate the time since the last reading and update the current time to be the new lastTime for the calculation in next iteration.

// Calculate time difference
unsigned long now = millis();
dt = (now - lastTime) / 1000.0; //Calculate delta time
lastTime = now;

Now we apply the filter equation onto the acquired readings. To find the most effective filtered angle, we apply the weight factor Alpha to the gyroscope rotation and accelerometer angle readings. The weight equation below gives a higher weight to the gyroscope data and lower weight to accelerometer data as you increase your alpha. Using this function is a common practice

angle = alpha * (angle + gyroRate * dt) + (1 - alpha) * accAngle;

Step 2: Apply the PID Controller.

Calculate the error using the measured angle we acquired from the previous step. Next, create the three terms of the PID controller as explained in the PID controller section, then multiply each term by its respective gain to get the motor control signal, output.

// Calculate PID
float error = setPoint - angle;
errorSum += error * dt;
float dError = (error - lastError) / dt;
float out = Kp * error + Ki * errorSum + Kd * dError;

Update the current error to be the previous error for the next iteration.

lastError = error;

Step 3:Send PWM signal to the motors.

Since output represents a duty cycle, it needs to follow the constraints of a duty cycle which ranges between -255 to 255 depending on the direction. Here we need to use the motor_ramp function we explained above to make sure the duty cycle calculated is not too low.

// Constrain PID output to the range [-255, 255]
int16_t output = constrain(round(motor_ramp(out)), -100, 100);

Now, the duty cycle is ready to be fed into the PWM signal generation function.

motor_pwm(0, -0.9 * output);  // Control motor 1
motor_pwm(1, output);   // Control motor 2

Note there is a factor multiplied by the output signal for one motor. This is a customizable factor, based on trial and error, which we used to get both motors to have the same output. You may need to adjust these factors to get satisfactory outputs from your motors.

The complete code is attached to this article. You will find there are some functions and lines of code which are not directly affecting the operation. These are used for debugging purposes. We recommend you use debugging statements and functions in order to monitor the status and functionality of your code.

Results

After integrating all the components together, now we have a fully-functioning self-balancing robot. HOORAY!

Farewell

This project showcases the use of PID in controlling motors to make a robot continuously balance itself on two wheels based on sensor data. We used the power of PSOC™ 6 AI kit for Arduino to combine all our logic for reading accelerometer and gyroscope data from the BMI270 sensor mounted on the PSOC™ 6 AI kit and filtering this data. We also delved deep into the concepts of PID and how to tune it. Finally, we used the Multi Half Bridge to control our two motors. Now, you can proudly say your robot can find its balance.

It was quite a journey; we learned a lot and we hope you did too! For more interesting projects visit the Infineon Team page; see you next time!