XOD and PID powered self-balancing mBot

Hi, everybody! It’s the third article about using a PID-controller in the XOD visual programming environment. We’ve already learned how to program the mBot platform by Makeblock into a hand follower robot and line follower robot. If you missed previous articles, then go back and start with the first. We don’t want to stop but make the most of a PID-controller and mBot platform.

In the last examples, we didn’t focus much on the Ki factor of the PID-controller. Therefore, for a new experiment, we are going to tell you how to make a self-balancing robot whose behavior directly depends on the Ki parameter.

As before, we’ve updated the gabbapeople/mbot-lib with the new nodes and patches. You can check updates at the XOD website and add it to your XOD workspace.

Turning mBot into the tumbler robot

First of all our robot has to undergo some changes. To make mBot feel the balance we need an IMU sensor. As we use the platform by the Makeblock, we add “Me 3 Axis Accelerometer and Gyro Sensor”. It’s based on a popular MPU6050 microchip and has a comprehensive set of functions. Also, it fits mBot, so we don’t need to think about attaching it to the chassis.

The second thing is that we disassemble mBot and swap its components to make it look like a tumbler. The IMU sensor takes its place at the top of the robot. The motors are designed to be mounted to the frame only in one place, however we can move them to a non-standard position. We power the robot with 3.7V Lipo battery that we fasten inside of the chassis.

Take a look at the sketch to see the changes in the robot’s design.

Tumbler sketch
Tumbler mBot

Reading the data from the IMU sensor

Again we start the XOD patch with reading the data from the sensor. For a tumbler robot, the patch is more complicated than in previous experiments. That’s all because of the “Me 3 Axis Accelerometer and Gyro Sensor” based on MPU6050 chip which communicates via I2C. By now, there is no officially supported node for this chip in the XOD environment. For this reason, we made our own node mpu6050-yrp-linear-accel which is an Arduino library wrapper.

mpu6050-yrp-linear-accel node

To make the program work we use a few Arduino libraries. These libraries should be added to the XOD IDE client. To add an Arduino library open the place where XOD is installed and copy the library folder to packages/xod-client-electron folder.

The list below shows which Arduino libraries you need to copy.

  • The I2Cdev and MPU6050 libraries by Jeff Rowberg. Last versions can be downloaded from the GITHUB.
  • The PinChangeInt library by Arduino. This library is used to control interrupts on pins on the controller board.

The MPU6050 chip has an INT pin which can be used to notify the controller about available data change. With this pin, you can get more accurate data from the sensor. “Me 3 Axis Accelerometer and Gyro Sensor” from the Makeblock supports the INT pin, and we will take advantage of it. Take a look at the right pin in the picture below.

“Me 3 Axis Accelerometer and Gyro Sensor” pinout

“Me 3 Axis Accelerometer and Gyro Sensor” has a white label and communicates via I2C. All four RJ25 ports on the mCore board are white labeled so the sensor can be connected to any of them. We connect the sensor to the RJ25 port 1 on the mCore board.

To attach the INT pin, we link the mpu6050-yrp-linear-accel node with the rj25port-1-yellow. The rj25port-1-yellow node will read data changes from RJ25 port 1 and send them to the mpu6050-yrp-linear-accel node.

Attaching the INT pin

What inputs and outputs does the mpu6050-yrp-linear-accel have? Let’s see.

  • INIT — the pin that initializes the sensor;
  • UPD — the pin that triggers a new read from the sensor;
  • GXoff, GYoff, GZoff — the gyroscope offsets;
  • AXoff, AYoff, AZoff — the accelerometer offsets.

This offset pins are optional and used to calibrate the sensor to improve the sensitivity and determine the mount position at the robot body. By default the offset values are set to 0.

  • ROLL, PITCH, YAW — the rotation angles around the X, Y, Z axes;
  • laX, laY, laZ — linear accelerations along X, Y, Z axes;
  • DONE — the pin that reports that data is read.

For the tumbler robot, we don’t need all output values. We will operate only with the ROLL value. Set the INIT pin value to on boot and the UPD pin value to Continuously.

Now, let’s add the watch node, link it with the ROLL and upload the patch. Turn on the debug mode to watch what the ROLL values are. To the test, we are going to deviate the robot from the balance position in different directions.

Test deviations
Roll values

The ROLL value approximately changes from 4.6 to 57. If the robot stands vertically the ROLL value is about 46. We get such a strange range of values because the sensor is not calibrated. So let’s do it.

Calibrating the sensor

How to calibrate the sensor?

We should specify the offset values GXoff, GYoff, GZoff, AXoff, AYoff, AZoff of gyroscope and accelerometer which are equal 0 now. To obtain this values we made the mpu6050-calibrate node.

mpu6050-calibrate node

The calibration can be performed once. Create a new empty temporary patch and add the mpu6050-calibrate node. Link six output pins with six watch nodes and set the INIT value to On boot.

Fix the robot in a vertical position and deploy the patch with the debug mode. Wait for a little, in less than a minute, the node outputs the necessary values. You can write them down not to forget.

Balanced position
Obtaining offsets

For our robot [GXoff, GYoff, GXoff, AXoff, AYoff, AZoff] offsets are [131, 29, -99, 1756, -2118, 1554]. Returning to the main patch we enter this values into the input pins of the mpu6050-yrp-linear-accel node and make a new debug test to see how the ROLL range changed.

Roll values after calibrating

Now rotating the robot 90 degrees in different directions, we get almost the perfect range of ROLL values from -89 to 89. That means we’ve calibrated the sensor well. The new range is very convenient for us to form the “error” concept.

Making the error concept

mBot is balancing about 87 degrees to the ground. The ROLL value for the balance position is about 3 degrees. We use this value as a set-point for the PID-controller and define the error.

Remind the formula

Any deviation from the balanced position makes an error. Look at the sketch below and check out the possible states.

Possible states
  • The robot balances.

The angle to the ground is about 87 degrees;

The ROLL value is about 3;

The error value equals 0;

The motors stand still.

  • The robot deviates to the right.

The angle to the ground is in the range [0,87] degrees;

The ROLL value is in the range [3,89];

The error value is negative;

The motors rotate counterclockwise.

  • The robot deviates to the left.

The angle to the ground is in the range [87,180] degrees;

The ROLL value is in the range [-89,3];

The error value is is positive;

The motors rotate clockwise.

Finishing the patch

Now finish the patch. Add the pid-controller node.

pid-controller node

Link the ROLL pin with the input of the pid-controller. Don’t forget to delete the watch node cause we don’t need it anymore. Link the UPD pin of the controller with the DONE pin of the sensor to make the pid-controller calculate the output only when the new data arrives.

We don’t configure the PID-controller coefficients yet, but, we can already set up the set-point in the TARG pin to 3.

With the last step we connect the pid-controller with the motors. We link the output of the pid-controller to the mbot-motors node through the clip node. The clip node controls the output and keeps its value in [-1,1] range.

mbot-motors node

If in your case the motors rotate in the wrong direction, swap the connections of the motors M1 and M2 on the mCore Board. Alternatively, change the sign of the value using the subtract node. The patch is done. Time to adjust the pid controller and watch the robot’s behavior.

Tuning the PID-controller

What Kp, Ki and Kd values mean for the tumbler bot?

For the tumbler bot, the process of the PID-controller tuning can be very tedious. It is more accurate than in our past examples, and you can spend a lot of time tuning it. Be patient, and you definitely find the proper coefficients.

Kp factor

The Kp coefficient determines the force with which the robot returns to the balancing position. With a low Kp value, the motors have no enough force to return the robot to the balancing state. A high Kp value greatly increases the oscillation of the robot from the balancing state. For the smooth oscillation Kp shouldn’t be too high, at the same time Kp shouldn’t be too low to give the robot power to move.

Let’s set the Kp to 0.05 and watch the robot behavior.

The 0.05 value of the Kp

We slightly hold the robot with our hand trying to feel the motors speed. This value seems too small. The motors don’t almost rotate. The robot do not have sufficient strength to correct itself. We set the Kp to 1.5.

The 1.5 value of the Kp

The 1.5 Kp value is too high. Such a fluent oscillation is hard to control. As before, we select the average value using the trial and error method. We set Kp to 0.155.

The 0.155 value of the Kp

Ki factor

In our previous articles, the main PID-controller parameters were Kp and Kd, while the Ki potential wasn’t fully unleashed. For a self-balancer robot, the main coefficients are Kp and Ki. It is possible to achieve the balance state using only these two parameters.

Ki determines the time for the robot to correct itself. The more the robot deviates, the more absolute error value accumulates, and more force is needed to return the robot to a balanced position. Physically, the Ki doesn’t affect the motors speed, but it affects motors acceleration. The higher the center mass is the higher the acceleration should be, otherwise the force to correct the robot position won’t be enough.

Let’s set the Ki to 0.05 and stop holding the robot by hand.

The 0.05 value of the Ki

mBot falls and we should increase Ki to make a faster response. We set it to 0.5.

The 0.5 value of the Ki

It’s better but we’re still not satisfied. We gradually increase the Ki value until the robot returns to balance position. So the Ki value for our robot is 1.195.

The 1.195 value of the Ki

Kd factor

By changing the Kd factor, you can fine-tune the sensitivity of the robot to the error. It is used to make the robot oscillations fast or slow. Our robot keeps the balance pretty nice with the 0 value of Kd. However, we increase it a little from 0 to 0.0075 to make oscillations sharper.

To show how the oscillations change when the Kd value changes, we decided to increase the Kd twice. Look at the robot’s behavior when the Kd is 0.015.

The 0.015 value of the Kd

A strong increase in oscillations can lead to the loss of the balance. We keep the Kd equal 0.0075.

Conclusion

In conclusion, we can only show you how the new self-balancing robot is riding around us.

Self-balancing mBot

You can see that it sometimes turns to the right or left. That happens because the motors are out of sync. If you want to repeat the experiment, you can fix it yourself but for our lesson it isn’t necessary.

The unusual task was simple. Thanks to the PID-controller, XOD, and the Makeblock constructor! See you later.