In this lesson, we will learn the principle of the Alter Dog robot's self-balancing function. You need to prepare the installed Alter Dog robot.
32.1 The realization principle of Alter Dog's self-balancing function
The self-balancing function of the Alter Dog robot is mainly realized based on its internal MPU6050 sensor. By acquiring the data of MPU6050, the robot's posture change is judged, and the servo control system is used to accurately drive the servo to adjust the corresponding posture to maintain the balance of the robot. In the process of data processing, we use the Kalman filtering algorithm, which is an algorithm that uses the linear system state equation to make the best estimation of the system state with the input and output observation data of the system. Since the observation data includes the influence of noise and interference in the system, the optimal estimation can also be regarded as a filtering process.
32.2 Learning the code program of this lesson
Here we use Subline IDE to view and edit the code program of this lesson, please see the content of lesson 2 for specific methods. The specific code and comments are as follows:
In the file manager of the MobaXterm terminal, find the directory:
adeept_alter/server
And open the code of this lesson: steady.py. This code program needs to be run in the server folder directory.
Import MPU6050 library.
Import Kaman filter library.
Instantiate MPU6050 object, the default I2C address is 0x68.
Instantiate the Kalman filter object on the XY axis, the Kalman filter can make the value read from the sensor smoother.
Set the maximum height and minimum height; and calculate the middle height by the maximum height and the minimum height; when the robot is on a horizontal plane, each leg of the robot is in the middle height state; the maximum and minimum heights are used to limit the range of motion of the robot's legs.
Since the automatic balance mode needs to continuously read information from the MPU6050; read the information once to calculate the change trend and then apply the new angle to the servo. These actions are called a cycle; so use this variable here to set the time interval between every two loops.
If the robot is pitched up and down at a certain angle when placed on a horizontal plane, we modify xMiddle; if the robot is tilted to the left and right at a certain angle when placed on a horizontal surface,we modify yMiddle; this can also be used in reverse. For example, when you want the robot to maintain a certain angle, you can adjust these two variables.
P value in simple PID controller, proportional parameter, increase this value if the robot moves slowly; If the robot does not stabilize and shakes too much, reduce this value.
Because this method is a closed-loop control; therefore, the accumulated error (error) is stored in these two variables; this allows the robot to remain level at any time even if it is on a constantly shaking panel.
This function is used to limit the size of a variable; enter the maximum and minimum values, and enter the variable that needs to be limited; the return value is between the maximum and minimum (including the maximum and minimum).
This function is used to adjust the angle of the robot: pitch is the pitch motion; roll is the roll motion; pIn is used to adjust the pitch axis motion, the larger the value of pIn, the more the robot heads up; rIn is used to adjust the roll axis motion, the larger the value of rIn, the more the robot tilt to the left.
Since the back and forth movement of the leg is not required in the automatic stabilization mode; therefore, the X-axis coordinate point of the end point of each leg is 0.
Calculate the coordinate value Y of the end point of each leg with pIn and rIn.
Apply the new coordinate values to the four legs.
It is a self-stabilization function; call it cyclically to realize the self-stabilization function.
Declared as a global variable.
If the I2C communication is disconnected, we re-instantiate the MPU6050 object.
Obtain the X-axis and Y-axis readings of MPU6050, and use Karman filter to eliminate the noise of MPU6050 readings.
Calculate the deviation value of each axis.
Calculate pitchValue and rollValue based on the deviation value, and apply the result to pitchRoll().
Delay for a while before proceeding to the next cycle.
32.3 Run the program of this course
1. Open the terminal software MobaXterm:
2. Log in to your Raspberry Pi (the way to log in to Raspberry Pi has been introduced in Lesson 1):
3. Enter the Course Code folder, this folder stores the sample code program to control the robot, enter the following command:
cd adeept_alter/server
4. Enter the command to display the contents of the current directory:
ls
5. steady.py is the sample code for this lesson, enter the command to run this program:
sudo python3 steady.py
6.After running the program successfully, you can observe the movement of the robot.