Two-wheel balance trolley in STM32 microcontroller
The senior here gives a comprehensive score for a topic (out of 5 points for each item)
- Difficulty factor: 4 points
- Workload: 4 points
- Innovation: 3 points
1 Introduction
Today we will introduce the two-wheel balance trolley module and algorithm principle based on STM32 single-chip microcomputer.
The car can realize the mobile phone APP to control forward, backward, left and right turn.
The HC-SR04 ultrasonic module can realize the following and obstacle avoidance modes.
Gravity sensing control can be realized using a smartphone.
2 Main components
- STM32F103C8T6 main control chip
- TB6612FNG motor drive module
- MPU6050 chip attitude monitoring sensor
- HC-05 Bluetooth module
- HC-SR04 ultrasonic module
3 Achieve the effect
Control board pin diagram
4 Design principles
4.1 PID algorithm
The PID algorithm has been around for 105 years, and it has been used in many places, such as cruise control in cars, temperature controllers for 3D printers, self-balancing cars, and flying vehicles. Before introducing the PID algorithm, we must first introduce the order response, which is the output curve response of the system when the input signal changes from zero hop to one, as shown in the following figure:
The black line in the figure is the input signal, after the input signal jumps directly from 0 to 1, the red line is the output response of the system, and you can see that it rises after a period of time. Gradually stabilized to around 1.
The quality of the system order response can be evaluated by some system performance indicators:
One. Overshoot: Overshoot
It is the ratio of the maximum offset of the response beyond the steady-state value to the steady-state value
Two. Rise time: Rise time
It is the time it takes for the response curve to rise from 10% to 90% of the steady-state value, and in the figure above, it refers to the rise time.
Three. Stable time: seeting time
It means that the response curve reaches near the stable value, such as plus or minus -5%, and does not exceed the minimum time outside this error range, that is, the setting time in the figure, after reaching the setting time, it can be considered that the system has basically reached a stable state.
Four. Steady-state error: steady-state error
Also called static error or static difference, it refers to the error between the steady-state value and the set value
If we want to control (as shown above) the unmanned car, to make it three meters more accurate than the car, then its steady-state error is the gap between its actual driving distance and the set value of three meters, the rising time refers to 10% of the distance value, that is, the driving time of 0.3 meters to 2.7 meters, and the stability time refers to the time it stabilizes around three meters. We need to design a feedback controller in order to make the performance curve of the original performance parameters not very good into the performance curve of the ideal state, that is, its rise time and settling time should be as small as possible, and the error between its steady-state value and its setpoint should be eliminated.
As shown in the above figure, the system response curve can be seen that its system response time is very long, and there is still a large error between the final steady-state value and its set value of three meters.
So to introduce the PID control algorithm, PID it is called this name, because it is composed of P, I, D three kinds of controllers, P controller is the most basic, on its basis can add i controller or D controller to form PI or PD controller, can also be used at the same time by three controllers, that is, the final PID controller, the following three controllers are introduced in detail.
P (proportional) controller is the simplest controller, P English full name Proportion (proportion), that is, the meaning of proportion, the basic idea of proportional controller is to the error of the system to present a scale factor kp, so as to obtain its control quantity output o, that is, the use of the positive and negative of the error and the size of the proportional response, scale factor kp, can get the corresponding o value of the positive and negative and magnitude, the size of the output and the size of the input error signal proportional relationship of a control. The formula is: O=Kp*e.
Assuming that the unmanned car is advanced by 3m, the duty cycle of the pwm wave output by the p controller is Duty_p, Duty_p = Kp*error, that is, Duty_p equal to the error multiplied by kp (scale factor). With the addition of the p controller, the system response curve can become the blue curve shown in the figure below.
It can be seen that the P controller reduces the rise time and steady-state error, but the system also has no overshoot itself and becomes a certain overshoot, that is, the P controller increases the overshoot of the system.
The above figure shows the three response curves corresponding to different scale factors Kp in the p controller, where blue is the case of p=5, red is the case of p=10, orange is the case of p=20, it can be seen that the larger the kp value, the faster the rise time, the smaller the steady-state error, but its overshoot will also increase. By simply changing the scale factor kp, the effect of system oscillation cannot be eliminated, and the larger the kp value, the amplitude of system oscillation is also increasing. That is, the correction of the error by the p controller will cause the system to produce an impulse beyond the steady-state value and produce oscillations when it is close to the ideal state. To solve the oscillation, it is necessary to correct the momentum of the system as it approaches the ideal state. (momentum = mass velocity), it is necessary to correct the speed, so that when the system is close to the ideal state, in order to make it easier for him to stabilize, we need to reduce the velocity value as much as possible, which requires the use of the derivative of the error, because the essence of the velocity is the derivative of the displacement, and the error here is the difference in the displacement value, so the velocity can also be considered as the derivative of the error (O=Kd* de/dt), we need to use it to further correct the output control quantity, that is, the introduction of differential control. It can use the derivative of the error, that is, the rate of change of the error, to correct the duty cycle of the PWM wave, so now duty_pd equal to the error multiplied by the scaling factor kp plus the change in error minus the previous error multiplied by a differential coefficient kd (Dudy_pd=Kperror+Kd(error-last_error)). Their response curves are shown in the following figure:
The blue line is the response curve of the P controller added before, and the red line is the response curve under the PD control, it can be seen that the use of the PD controller has reduced the overshoot a lot compared to before, and the stabilization time has also been reduced. In fact, the proportional controller is a kind of post-event adjustment, it is adjusted after the error, the differential controller is adjusted in advance, that is, advanced predictive control, it has a tendency to reduce the error, that is, this time the error is less than the previous error, the addition of the D controller will make its output smaller, thereby avoiding overshoot caused by excessive control, when the error has a tendency to increase, that is to say, this time the error is greater than the last error, the D controller will make the output larger, thereby suppressing the growth trend of error, speed up the movement of the system and reduce the adjustment time. At the same time, it can also be found that at this time the system still has steady-state error, in order to eliminate this we need to introduce another controller, that is, the integral controller to improve the control system, it can record its own previous moments of error, especially repeated, and the value is relatively fixed error, especially steady-state error, it can add up these errors, when it reaches a certain threshold, that is, large enough, the system will make some compensation or correction. So at this time, the output of the Pi controller is the duty cycle of the PWM wave, (Dudy_pi=Kperror+Ki∑error)Dudy_pi equal to the error multiplied by the scale factor kp plus the sum of the error of the previous period of time multiplied by the integration factor Ki, the following figure curve is the response curve under three different controllers:
The green line and blue line are the response curves corresponding to the previous PD and P controllers, and the red line is the response curve corresponding to the PID controller. Therefore, the main function of the i integration controller is to eliminate the static error, so that there is no error between the final steady-state value of the system and the set value, and the strength of the integration effect also depends on the integration time constant, if the integration time constant is larger, the slower the integration term, the weaker the control adjustment efficiency. Considering all the factors comprehensively, considering the rate of change of the error and the factors that eliminate the error, we can use all three controllers, that is, proportional integration and differential operations on the error at the same time, and its system response is shown in the figure below:
The addition of PID closed-loop control system at this time not only has a fast school time, but also has no overshoot and no steady-state error.
To make a simple summary, the main function of PID is to set the value and the actual value to do the difference, and their error is input to the proportional, integral and differential controllers. The proportional controller is to multiply the error by a constant, the integral controller is to accumulate the error for a period of time, the differential is to calculate the rate of change of the error, the sum of the outputs of the three is accumulated, the final PID output result is obtained and output to the control object, that is, the control system to be improved, thus forming a closed-loop circuit.
4.2 HC-SR04 ultrasonic module
Brief introduction
HC-SR04 ultrasonic module is commonly used in robot obstacle avoidance, object ranging, liquid level detection, public security, parking lot detection and other places. The HC-SR04 ultrasonic module is mainly composed of two general-purpose piezoelectric ceramic ultrasonic sensors and peripheral signal processing circuits. As shown in the picture:
Two piezoceramic ultrasonic sensors, one for emitting ultrasonic signals and one for receiving the reflected ultrasonic signals. Since the emitted signal and the received signal are relatively weak, it is necessary to increase the power of the emitted signal through the peripheral signal amplifier and amplify the reflected signal to transmit the signal to the single-chip microcomputer more stably. The overall circuit of the module is shown in the figure:
Module parameters
(1) Main electrical parameters of the module
- Operating voltage: DC-5V
- Quiescent current: less than 2mA
- Level output: 5V high
- Level output: bottom 0V
- Sensing angle: not more than 15 degrees
- Detection distance: 2cm-450cm
- High accuracy up to 0.2cm
(2) Module pins
The ultrasonic module has 4 pins, namely Vcc, Trig (control end), Echo (receiving end), GND; Among them, VCC and GND are connected to the 5V power supply, Trig (control end) controls the ultrasonic signal emitted, and Echo (receiving end) receives the reflected ultrasonic signal. The module is shown in the figure
4.3 TB6612FNG motor drive module
Brief introduction
TB6612FNG is a new type of drive device, can independently control two DC motors, it has a high degree of integration, while providing sufficient output capacity, operating performance and energy consumption also has advantages, so in the integrated, miniaturized motor control system, it can be used as an ideal motor drive device.
The schematic diagram is as follows:
TB6612FNG driver interface in detail
The TB6612FNG driver is shown in the following figure
Interface Description:
- VM: Motor drive power input (2.74-10.8V)
- VCC: Logic Level Input (5.0V)
- AO1 and AO2: Connect DC motor A
- BO2 and BO1: Connect DC motor B
- PWMA: Controls the speed of motor A
- AIN1 and AIN2: Control motor A (stop, forward, reverse)
- STBY: normal operation/standby control end PWMB: controls the speed of motor B
- BIN1 and BIN2: Control motor B (stop, forward, reverse)
- GND: Grounded
AIN1 and AIN2 are used to control motor A stop, forward rotation, and reverse, and the truth table is as follows
AIN1001AIN2010 state stops, reverses, and the truth table of BIN1 and BIN2 is the same as above
4.4 MPU6050 chip attitude monitoring sensor
MPU6050
The MPU-60x0 is the world's first 9-axis motion processing sensor. It integrates a 3-axis MEMS gyroscope, a 3-axis MEMS accelerometer, and an expandable digital motion processor DMP (Digital Motion Processor) that can be connected to a third-party digital sensor, such as a magnetometer, using an I2C interface. It can be extended to output a 9-axis signal via its I2C or SPI interface (SPI interface is only available on the MPU-6000). The MPU-60x0 can also be connected to non-inertial digital sensors, such as pressure sensors, via its I2C interface.
The MPU-60x0 uses three 16-bit ADCs each for the gyroscope and accelerometer to convert its measured analog quantities into outputs of digital quantities. In order to accurately track fast and slow movements, the measurement range of the sensor is user-controllable, the gyroscope can measure ±250, ±500, ±1000, ±2000°/s (dps), and the accelerometer can measure the range of ±2, ±4, ±8, ±16g. An on-chip FIFO of 1024 bytes helps reduce system power consumption. Communication with all device registers uses either a 400kHz I2C interface or a 1MHz SPI interface (SPI is only available for the MPU-6000). For applications requiring high-speed transmission, 20MHz SPI can be used for register reads and interrupts. In addition, a temperature sensor and oscillator that changes only ±1% in the operating environment are embedded on-chip.
5 parts of core code
/************************** .h文件******************************/
#ifndef _pid_H
#define _pid_H
#define MODEL_P 1
#define MODEL_PI 2
#define MODEL_PID 3
typedef struct
{
u8 choose_model; //使用哪个模式调节，以方便分布调试
float curr; //当前值
float set; //设定值
float En; //当前时刻误差值
float En_1; //前一时刻误差值
float En_2; //前二时刻误差值
float Kp; //比例系数
float T; //每隔T控制器输出一次PID运算结果
u16 Tdata; //判断PID周期到没到
float Ti; //积分时间常数
float Td; //微分时间常数
float Dout; //增量PID计算本次应该输出的增量值-本次计算的结果
float OUT0; //一个维持的输出，防止失控
short currpwm; //当前的pwm宽度
u16 pwmcycle; //pwm周期
}PID;
extern u8 STATUS;
extern PID pid;
void PID_Init(void); //增量式PID初始化
void pid_calc(void); //pid计算 并输出
/************************** .c文件******************************/
#endif
#include "pid.h"
#include "PWM_Config.h"
#include "USART_Config.h" //USART设置
PID pid;
void PID_Init() //
{
pid.choose_model = MODEL_PID;
pid.T=330; //采样周期，定时器使用1ms，则最小执行PID的周期为330ms
pid.set =280; //用户设定值
pid.Kp=0.5; //比例系数
pid.Ti=40; //微分系数常数
pid.Td=10; //积分时间常数
pid.OUT0=0; //一个维持的输出
pid.pwmcycle = 330; //PWM的周期
}
void pid_calc()
{
float dk1;float dk2;
float t1,t2,t3;
if(pid.Tdata < (pid.T)) //最小计算周期未到
{
return ;
}
pid.Tdata = 0;
pid.En=pid.set-pid.curr; //本次误差
dk1=pid.En-pid.En_1; //本次偏差与上次偏差之差
dk2=pid.En-2*pid.En_1+pid.En_2;
t1=pid.Kp*dk1; //比例
t2=(pid.Kp*pid.T)/pid.Ti; //积分
t2=t2*pid.En;
t3=(pid.Kp*pid.Td)/pid.T; //微分
t3=t3*dk2;
switch(pid.choose_model)
{
case MODEL_P: //仅用P
pid.Dout= t1;
printf("使用P运算\r\n") ;
break;
case MODEL_PI: //仅用PI
pid.Dout= t1+t2;
printf("使用PI运算\r\n") ;
break;
case MODEL_PID: //用PID
pid.Dout= t1+t2+t3;
printf("使用PID运算\r\n") ;
break;
}
pid.currpwm+=pid.Dout; //本次应该输出的PWM
printf("PID算得的OUT:\t%d\r\n",(int)pid.currpwm) ;
if(pid.currpwm>pid.pwmcycle) //确保值在0-pid.pwmcycle之间
{
pid.currpwm=pid.pwmcycle;
}
if(pid.currpwm<0)
{
pid.currpwm=0;
}
printf("实际输出使用的OUT:\t%d\r\n",(int)pid.currpwm) ;
pid.En_2=pid.En_1;
pid.En_1=pid.En;
}
The full code can be collected for free by joining the group.
The learning road of embedded IoT is very long, and many people miss out on high-paying offers because the learning route is wrong or the learning content is not professional enough. But don't worry, I've put together a list of more than 150 GB of learning resources for you, basically covering everything about embedded IoT learning. Click the link below to receive learning resources for 0 yuan, making your learning road smoother! Remember to like, follow, favorite, and retweet!
Click here to find the assistant 0 yuan to receive: scan the code to enter the group to receive the information