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.
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.
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” 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.
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.
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.
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.
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.
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.
Any deviation from the balanced position makes an error. Look at the sketch below and check out the 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.
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.
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.
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 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.
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.
mBot falls and we should increase Ki
to make a faster response. We set it to 0.5.
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.
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.
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.
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.