Incremental PID for DC geared motor Easy-to-understand version
For an undergraduate student majoring in electronics at my double-african school who has never attended self-control, this thing is really too difficult. I studied an A4950 driver before, which is cheap, high driving capability, high safety, and high convenience. The words are easier to use than L298N. After I finished writing, I sent it out. After reading it, one of my teachers said that he didn't see it. I should get a PID control. Okay. I will do it. Why is it easy to understand Version, this, because I don’t understand it very deeply. If there is a mistake, I hope that the big guy will point him to Haihan. I believe it should be enough for most students. This article is about a motor. This article is researched in the home of the balance car
1. Brief introduction to the principle of DC motor incremental PID closed-loop control
We generally divide PID into two types, one is called position PID and the other is speed PID. Position PID is generally used for inverted pendulum, balance trolley, etc., to make a parameter accurately reach a specified static state, such as making a motor accurate Turn 90 degrees, and the incremental PID is used to keep a dynamic variable as stable as possible at a certain target value, so we want the car to drive at a uniform speed as accurately as possible, which is to make the speed of each wheel stabilize at a target The value, how is it achieved? I divided it into four steps. To
understand this flowchart, you must at least know:
(1) The basic principle of the encoding motor, the encoder of the encoding motor, the corresponding speed will be different within the same time. Output different pulses
(2) The single-chip microcomputer obtains the speed by reading the rising edge or falling edge, or the rising edge and falling edge of the pulse output by the encoder (a continuous pulse is a square wave).
Okay, we will explain this diagram next.
Step 1: The single-chip microcomputer obtains the speed through the encoder of the DC geared motor, and obtains the error between the current speed and the target speed.
Step 2: According to the error, the PI controller will calculate a PWM value for adjusting the DC geared motor to the target speed
. Step 3: Output the calculated PWM value to the DC motor, and adjust the speed of the DC motor.
Step 4: Read the speed of the DC motor again and send it to the microcontroller
PI controller: a function, the input is the error between the current speed and the target speed, and the output is the PWM value needed to adjust the speed
Next, let me introduce a living fairy with a uniform speed of the car, discrete PID formula
PWM+=Kp× [e (k) -e (k-1)] +Ki×e(k)+Kd× [e (k) -2e (k-1) + e (k-2)]
Look at the formula and get dizzy!? Don’t go, don’t go, we won’t use this complicated formula at the end.
Let me first introduce what its parameters represent.
PWM: The output increment is the output value
The three parameters of Kp, Ki, and Kd need to be adjusted accordingly. According to continuous testing and adjustment, a set of optimal values can be obtained. What!? The four words are perfunctory? Don't go, don't go, there will be examples Well, you must understand, you must understand
e(k): this time error
e(k-1): Last error
e(k-2): last time error
The ancestors of the formula we are going to use are finished, the formula we actually use can be simplified to
PWM+=Kp× [e (k) -e (k-1)] +Ki×e(k)
This simple and short formula is the key to unlock the treasure of the car at a constant speed
The C language implementation code is as follows
int Incremental_PI (int Encoder,int Target)
{
static float Bias,PWM,Last_bias;
Bias=Target-Encoder; //计算偏差
PWM+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; //增量式PI控制器
Last_bias=Bias; //保存上一次偏差
return PWM; //增量输出
}
Let's explain in detail line by line
int Incremental_PI (int Encoder,int Target)
Define a function whose return value is an integer calledIncremental_PIThe input is two integer variables Encoder, Target (Encoder represents the current speed, Target represents the target speed)
Bias=Target-Encoder;
Calculate deviation target value minus current value
static float Bias,PWM,Last_bias;
Define three global floating-point static variables, Bias (this deviation), PWM (output increment), Last_bias (last error)
PWM+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;
Calculate the PWM value needed to adjust the deviation
Last_bias=Bias;
Primary deviation on storage
return PWM;
Return the PWM value to be output
2. Experiment preparation
1. Arduino mega 2560
2.12V power supply, one I use is a model airplane battery
3. DC motor drive A4950
link: A4950 drive detailed explanation
4. Encoding motor
5. Breadboard
6. Several Dupont lines The
wiring diagram is as follows:
| mega 2560 pull leg | Peripheral pins |
|---|---|
| 2 | Encoder Phase A |
| 50 | Encoder Phase B |
| 5 | A4950 driver AIN1 |
| 6 | A4950 driver AIN2 |
| 5V | Breadboard+ |
| GND | Breadboard- |
| Encoding motor pin | Connected pin |
|---|---|
| Motor line﹢ | A4950 drives AOUT1 |
| Motor wire- | A4950 drives AOUT2 |
| Encoder 5v | Breadboard+ |
| Encoder GND | Breadboard- |
| Encoder Phase A | mega2560 pin 2 |
| Encoder Phase B | mega2560 pin 50 |
| A4950 driver | Connected pin |
|---|---|
| VCC | Breadboard﹢ |
| GND | Breadboard- |
| VM | Drive 12V power supply+ |
| AIN1 | mega2560 pin 5 |
| AIN2 | mega2560 pin 6 |
| AOUT1 | Encoding motor motor line+ |
| AOUT2 | Encoding motor motor line- |
| 12V drive power | Connected pin |
|---|---|
| positive electrode | A4950VM |
| GND | Breadboard- |
3. Preliminary knowledge of experiment
1. Encoder AB phase
The A phase and B phase of the encoder output two pulses with phase difference, which are actually two square waves. Then how do we output square waves from the encoder A phase and B phase distinguishWhat we believePros and cons
For example, our counter counts by detecting the rising edge of the A phase output square wave. When we detect the rising edge of the A phase output, we find that if the B phase output is high at this time, then we can It is assumed that the motor is rotating forward at this time. When the rising edge of phase A is detected, if the output of phase B is low, then we can determine that the motor is reverse
If we only detect the number of occurrences of an edge of a phase in a unit time, we call this speed measurement method the M speed measurement method. If we detect both the rising and falling edges of the AB phase, it can achieve four times the accuracy. It is called four-frequency speed measurement, but the Arduino mega 2560 has only 6 interrupt IO ports, and there are also IO ports shared with the I2C communication port and the Serial1 serial port, so we only use four interrupt IO ports for each interrupt IO port. Corresponding to the encoder of a motor, but we can detect the rising and falling edges of phase A using two frequency division speed measurement 
2. Timing internal interrupt
If you want to use Arduino as a four-wheeled PID trolley, you need at least four interrupt IO ports, Generally speaking, we all use Mega2560. The following is the corresponding pin number and interrupt number of the arduino series MCU
. The rising and falling edges of the pulse within the set time need to be counted, and without occupying the CPU, we need to use the timing internal interrupt Of course, the biggest advantage of arduino is convenience. We don’t need to configure a lot of registers and a lot of modes like using 32. We use the mega2560 internal timer library written for us by a foreign guy.
#include <FlexiTimer2.h> //定时中断
The configuration is also very simple. Just write two lines in setup().
FlexiTimer2::set(5, control); //5毫秒定时中断函数,每5s执行一次control函数
FlexiTimer2::start (); //中断使能
It's as if you understand the basics, then we start
4. Experimental code
The function of the following code is to input a target speed Velocity from the serial port, so that the speed of the motor can be maintained at the target value as accurately as possible. Velocity represents the number of rising and falling edges within a specified time. The time set in the internal timing interrupt, also known as the sampling period
unsigned int Motor_AIN1=5; //控制A电机的PWM引脚 一定改成自己用的
unsigned int Motor_AIN2=6;
String Target_Value; //串口获取的速度字符串变量
int value; //用于存储通过PI控制器计算得到的用于调整电机转速的PWM值的整形变量
#include <FlexiTimer2.h> //定时中断头文件库
/***********编码器引脚************/
#define ENCODER_A 2 //编码器A相引脚
#define ENCODER_B 50 //编码器B相引脚
int Velocity,Count=0; //Count计数变量 Velocity存储设定时间内A相上升沿和下降沿的个数
float Velocity_KP =7.2, Velocity_KI =0.68,Target=0;//Velocity_KP,Velocity_KI.PI参数 Target目标值
/*********** 限幅************
*以下两个参与让输出的PWM在一个合理区间
*当输出的PWM小于40时电机不转 所以要设置一个启始PWM
*arduino mega 2560 单片机的PWM不能超过255 所以 PWM_Restrict 起到限制上限的作用
*****************************/
int startPWM=40; //初始PWM
int PWM_Restrict=215; //startPW+PWM_Restric=255<256
void setup()
{
Serial.begin(9600); //打开串口
Serial.println("/*****开始驱动*****/");
pinMode(ENCODER_A,INPUT); //设置两个相线为输入模式
pinMode(ENCODER_B,INPUT);
pinMode(Motor_AIN1,OUTPUT); //设置两个驱动引脚为输出模式
pinMode(Motor_AIN2,OUTPUT);
FlexiTimer2::set(5, control); //5毫秒定时中断函数
FlexiTimer2::start (); //中断使能
attachInterrupt(0, READ_ENCODER_A, CHANGE); //开启对应2号引脚的0号外部中断,触发方式为CHANGE 即上升沿和下降沿都触发,触发的中断函数为 READ_ENCODER_A
}
void loop()
{
while(Serial.available()>0) //检测串口是否接收到了数据
{
Target_Value=Serial.readString(); //读取串口字符串
Target=Target_Value.toFloat(); //将字符串转换为浮点型,并将其赋给目标值
Serial.print("目标转速频率:"); //串口打印出设定的目标转速
Serial.println(Target);
}
}
/**********外部中断触发计数器函数************
*根据转速的方向不同我们将计数器累计为正值或者负值(计数器累计为正值为负值为计数器方向)
*只有方向累计正确了才可以实现正确的调整,否则会出现逆方向满速旋转
*
*※※※※※※超级重点※※※※※※
*
*所谓累计在正确的方向即
*(1)计数器方向
*(2)电机输出方向(控制电机转速方向的接线是正着接还是反着接)
*(3)PI 控制器 里面的误差(Basi)运算是目标值减当前值(Target-Encoder),还是当前值减目标值(Encoder-Target)
*三个方向只有对应上才会有效果否则你接上就是使劲的朝着一个方向(一般来说是反方向)满速旋转
我例子里是我自己对应好的,如果其他驱动单片机在自己尝试的时候出现满速旋转就是三个方向没对应上
下列函数中由于在A相线上升沿触发时,B相是低电平,和A相线下降沿触发时B是高电平是一个方向,在这种触发方式下,我们将count累计为正,另一种情况将count累计为负
********************************************/
void READ_ENCODER_A()
{
if (digitalRead(ENCODER_A) == HIGH)
{
if (digitalRead(ENCODER_B) == LOW)
Count++; //根据另外一相电平判定方向
else
Count--;
}
else
{
if (digitalRead(ENCODER_B) == LOW)
Count--; //根据另外一相电平判定方向
else
Count++;
}
}
/**********定时器中断触发函数*********/
void control()
{
Velocity=Count; //把采用周期(内部定时中断周期)所累计的脉冲上升沿和下降沿的个数,赋值给速度
Count=0; //并清零
value=Incremental_PI_A(Velocity,Target); //通过目标值和当前值在这个函数下算出我们需要调整用的PWM值
Set_PWM(value); //将算好的值输出给电机
}
/***********PI控制器****************/
int Incremental_PI_A (int Encoder,int Target)
{
static float Bias,PWM,Last_bias; //定义全局静态浮点型变量 PWM,Bias(本次偏差),Last_bias(上次偏差)
Bias=Target-Encoder; //计算偏差,目标值减去当前值
PWM+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; //增量式PI控制计算
if(PWM>PWM_Restrict)
PWM=PWM_Restrict; //限幅
if(PWM<-PWM_Restrict)
PWM=-PWM_Restrict; //限幅
Last_bias=Bias; //保存上一次偏差
return PWM; //增量输出
}
/**********实际控制函数*********/
void Set_PWM(int motora)
{
if (motora > 0) //如果算出的PWM为正
{
analogWrite(Motor_AIN1, motora+startPWM); //让PWM在设定正转方向(我们认为的正转方向)正向输出调整
analogWrite(Motor_AIN2, 0);
}
else if(motora == 0) //如果PWM为0停车
{
analogWrite(Motor_AIN2, 0);
analogWrite(Motor_AIN1, 0);
}
else if (motora < 0) //如果算出的PWM为负
{
analogWrite(Motor_AIN2, -motora+startPWM); //让PWM在设定反转方向反向输出调整
analogWrite(Motor_AIN1, 0);
}
}
The effect of uploading code is as follows
. How do you tell if it is a uniform speed?
If you don’t have a host computer oscilloscope,
you can print Volocity on the serial port. It’s
silly but it can be used.
void loop()
{
while(Serial.available()>0)
{
Target_Value=Serial.readString();
Target=Target_Value.toFloat();
Serial.print("目标转速频率:");
Serial.println(Target);
}
Serial.println(Velocity); //这句是新加的
}
For example, if I input a 10, I can see that the output speed will fluctuate around 10. I cut a picture that is shaking. In fact, the output speed remains at 10 most of the time, and the output speed will not exceed [9.11]. It will float in this range, which is 1 up and down.
If you have a host computer oscilloscope, it will be more comfortable to see. The
yellow line is the PWM value for adjusting the speed output, the white line is the actual value, and the red line is the target value. The
red line basically coincides with the white line. I can’t see it.
Let’s take a closer look at the deviation between the white line and the red line, which does not exceed 1. The four wheels are adjusted to this level. The actual effect is quite good. I think it is enough. Of course, you can adjust it.
5.Kp, Ki parameter adjustment
Okay, then the key question is how to adjust the parameters of Kp and Ki.
In fact, because we only use two parameters, it is better to adjust
1. Generally speaking, the first parameter Kp keeps the actual value at a stable floating point. The effect of a fixed value up and down. The size of the Kp parameter is related to the ability of the motor drive and the voltage value of the drive power supply. The stronger the drive ability, the larger the Kp. The situation is diverse. Only keep trying to see which parameter has the most effect. Well, the oscillation amplitude is the smallest and the number of oscillations is the least.
Personally feel that the adjustment method is different from the position PID. I don’t want to do too many complicated explanations.
But I can still give you some suggestions. Take the parameters in my example as an example, I use It is A4950, 12V drive power supply, the parameter used is Velocity_KP = 7.2, if you use a drive combination that is stronger than mine (the drive module is better than mine, and the drive voltage is higher than mine), then you can use the parameters of my example Start to try up little by little, by the way, the driving ability of L298N is similar to that of A4950.
2. The second parameter Ki plays the role of making this fixed value the target value. It is used to eliminate the steady-state error, also known as the static error. How does it perform specifically?
I used the parameters in the above example.
Velocity_KP =7.2, Velocity_KI =0.68
Next I changed Ki to 0
// An highlighted block
Velocity_KP =7.2, Velocity_KI =0
There will be the following situation. Although the white line also fluctuates at a fixed value, this fixed value is obviously deviated from our target value, that is, the red line. If you don't understand this principle, you can search for the steady-state error, starting from Ki This is to eliminate the effect of steady-state error.
Let’s assign Velocity_KI to 0.68 again.
You can see that the red and white lines start to be "entangled" again.
In the PI control example where the car wants to be at a constant speed, this Ki does not need to be too large.
6. Postscript
In the last parameter adjustment, I have read a lot before, this article on parameter adjustment, there are a lot of formulas and so on, I personally feel that it is not very easy to understand, and it is basically used for the first type of position PID Yes, and then I don’t feel as happy as if I tried it directly.I probably adjusted Kp first to stabilize the system, and then Ki let the system eliminate the static error and stabilize it at the stable value.The first time I wrote the PID, I was a bit scared, afraid of myself Which piece of understanding is not in place, misleading everyone, if there is something wrong with the writing, I hope you can criticize and correct it, and hope to help you