Using stm32 to implement PID control of motor

 

The internal structure and working principle of DC motors have actually been taught in high school physics textbooks. Here we mainly discuss how to drive a DC motor using a microcontroller and a motor driver module.

 

The microcontroller model used in this article is stm32f103rct6, and STM32F103C8T6 can also be used.

The motor driver selects TB6612

DC motor. This encoder motor with Hall sensor is selected, 12V, reduction ratio 1/30, speed 330rpm.

In order to facilitate observation and operation, a 0.96-inch OLED is used .

 

wiring

 

 

 

The driver of TB6612 is very simple. It uses two common GPIO outputs of high and low levels to control the forward and reverse rotation of the motor, and then uses a multiplexed timer IO to generate a PWM to control the motor speed.

The motor.c part of the code is as follows:

 

#include "motor.h"

/**

  * @brief  电机方向控制引脚设置

  * @param  None

  * @retval None  

  */

static void motor_gpio_Init(void)

{

  GPIO_InitTypeDef GPIO_InitStructure;

  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能PB端口时钟

  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13; //端口配置

  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;      //推挽输出

GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;     //50M

  GPIO_Init(GPIOB, &GPIO_InitStructure);       //根据设定参数初始化GPIOB 

}

 

/**

  * @brief  定时器初始化

  * @param  arr：自动重装值,设为一个时钟频率的最大值

  * @param  psc： 预分频值

  * @retval None  

  */

void Motor_PWM_Init(u16 arr,u16 psc)

{

GPIO_InitTypeDef GPIO_InitStructure;

TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;

TIM_OCInitTypeDef  TIM_OCInitStructure;

    motor_gpio_Init();

RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);// 

  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA , ENABLE);  //使能GPIO外设时钟使能

   //设置该引脚为复用输出功能,输出TIM1 CH1 CH4的PWM脉冲波形

GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11; //TIM_CH1 //TIM_CH4

GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;  //复用推挽输出

GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

GPIO_Init(GPIOA, &GPIO_InitStructure);

 

TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值  

TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值  不分频

TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim

TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式

TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位

 

TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式1

TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能

TIM_OCInitStructure.TIM_Pulse = 0;                            //设置待装入捕获比较寄存器的脉冲值

TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;     //输出极性:TIM输出比较极性高

TIM_OC4Init(TIM1, &TIM_OCInitStructure);  //根据TIM_OCInitStruct中指定的参数初始化外设TIMx

 

    TIM_CtrlPWMOutputs(TIM1,ENABLE); //MOE 主输出使能

 

TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable);  //CH4预装载使能  

 

TIM_ARRPreloadConfig(TIM1, ENABLE); //使能TIMx在ARR上的预装载寄存器

 

TIM_Cmd(TIM1, ENABLE);  //使能TIM1

} 

 

 

Just use a function, the input is a signed integer variable, the positive and negative signs represent the selected direction, and the absolute value represents the duty cycle.

 

/**

  * @brief  电机输出控制

  * @param  motor_pwm: 占空比 0-7200

  * @retval None  

  */

void Set_Pwm(int motor_pwm)

{

  if(motor_pwm>0)

  {

  BIN1=1;

      BIN2=0; //前进 

  }

 

 else if(motor_pwm<0)

{

BIN1=0;

BIN2=1; //后退

}

else  //停止

{

BIN1=0;

BIN2=0; 

}

PWMB=myabs(motor_pwm);

TIM_SetCompare4(TIM1, PWMB);

}

 

The first line of the OLED displays the running status, ON or OFF, and the second line displays the motor PWM value, + represents forward rotation, - represents reverse rotation, press K1 to switch the motor rotation direction. Press K2 to start/stop the motor.

gitee-PID motor control source code based on stm32

 

 

The encoder used is a Hall sensor, with two Hall elements placed 90 degrees apart. The speed reading methods of the encoder motor are divided into timer input capture method and external interrupt method from the microcontroller reading method, double frequency and quadruple frequency from the encoder principle, and M method speed measurement and T method speed measurement from the speed calculation method.

Let's take a brief look at M method speed measurement and T method speed measurement .

 

M method speed measurement : In a fixed timing cycle, the number of encoder pulses during this period is counted to calculate the speed value.

The calculation formula for the speed n is:

  n = M 0 / ( C ∗ T 0 )   

 

C: Total number of encoder pulses per turn

T0: Counting period, unit: second (s)

M0: Number of encoder pulses counted within the counting period

Assume that it is known that the encoder needs 100 pulses to rotate one circle, and 20 pulses are measured in 100ms. Calculate by the formula n = 20/(100*0.1)=2 (circles/second)

 

T method speed measurement : This method is to establish a high-frequency pulse of known frequency and count it.

The formula for calculating the speed n is:

  n = F 0 / ( C ∗ M 1 )

 

C: total number of pulses per single turn of the encoder

F0: frequency of high-frequency pulses

M1: number of turns per pulse

 

The T method uses a known pulse to measure the time between two encoder pulses to calculate the speed.

 

Assuming that the encoder requires 100 pulses (C=100) to make one revolution, then one pulse makes 1/100 revolution, which takes 20ms, or 50hz, and the speed is 0.5 revolutions/second.

 

The M method and T method of speed measurement are used to solve the problem of speed calculation. The following will discuss the timer input capture method and external interrupt method.

 

First, two orthogonal signals are input from GPIO to the filter, and then edge detection and polarity selection are performed, and then connected to the encoder interface inside the timer.

The speed measurement using the timer input capture method actually utilizes the encoder interface function of the stm32 timer:

the encoder interface can receive the signal of the incremental (orthogonal) encoder, and automatically control the CNT to increase or decrease according to the orthogonal signal pulses generated by the encoder rotation, thereby indicating the position, rotation direction and rotation speed of the encoder.

 

Below is the relationship between the counting direction and the encoding signal:

The following are some codes of encoder.c:

 

/**

  * @brief  编码器初始化,使用定时器输入捕获法

  * @param  None

  * @retval None 

  */

void Encoder_Init(void)

{

RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);

RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);

 

    GPIO_InitTypeDef GPIO_InitStructure;

GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;

GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;

GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

GPIO_Init(GPIOB, &GPIO_InitStructure);

 

TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;

TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;

TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;

TIM_TimeBaseInitStructure.TIM_Period = 65536 - 1; //ARR

TIM_TimeBaseInitStructure.TIM_Prescaler = 1 - 1; //PSC

TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;

TIM_TimeBaseInit(TIM4, &TIM_TimeBaseInitStructure);

 

    TIM_ICInitTypeDef TIM_ICInitStructure;

TIM_ICStructInit(&TIM_ICInitStructure);

TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;

TIM_ICInitStructure.TIM_ICFilter = 0xF;

 

TIM_ICInit(TIM4, &TIM_ICInitStructure);

TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;

TIM_ICInitStructure.TIM_ICFilter = 0xF;

 

TIM_ICInit(TIM4, &TIM_ICInitStructure);

/*TI1和TI2都计数，上升沿计数*/

TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);

 

TIM_Cmd(TIM4, ENABLE);

}

/**

  * @brief  获取定时器的计数值

  * @param  None

  * @retval None 

  */

int16_t Encoder_Get(void)

{

int16_t Temp;

Temp = TIM_GetCounter(TIM4);

TIM_SetCounter(TIM4, 0);

return Temp;

}

 

 

In addition, a timer is used to set the acquisition speed:

 

 /**

  * @brief  定时器中断，每100ms更新一次速度

  * @param  None

  * @retval None 

  */

  

void TIM2_IRQHandler(void)

{

if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)

{

Speed = Encoder_Get();

TIM_ClearITPendingBit(TIM2, TIM_IT_Update);

}

}

 

Use the jump signal of the microcontroller pin to trigger the interrupt, and then judge the level of the two encoder pins in the interrupt to increase or decrease the count value. We stipulate that the forward count value is ++, and the reverse forward count value is -.

Use 4 times frequency counting, that is, the rising edge and falling edge of phase A and the rising edge and falling edge of phase B all trigger the interrupt.

 

When the motor reverses, the phases of the A and B phase signals are 90 degrees different from those during forward rotation. In other words, during forward rotation, phase A triggers the rising edge first, while during reverse rotation, phase B triggers the rising edge first.

 

 

Some of the code is as follows:

 

/**

  * @brief  编码器初始化,使用外部中断法

  * @param  None

  * @retval None 

  */

void Encoder_Init(void)

{

GPIO_InitTypeDef GPIO_InitStruct;

EXTI_InitTypeDef EXTI_InitStruct;

NVIC_InitTypeDef NVIC_InitStruct;

RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);

GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IPD;

GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;

 

GPIO_Init(GPIOB,&GPIO_InitStruct);

RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);

//设置IO口与中断线的映射关系

GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource6);

GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource7);

//4.初始化线上中断

EXTI_InitStruct.EXTI_Line =  EXTI_Line6 | EXTI_Line7;

EXTI_InitStruct.EXTI_LineCmd = ENABLE;

EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt;

EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Rising_Falling;//跳变沿触发

EXTI_Init(&EXTI_InitStruct);

//5.配置中断分组

NVIC_InitStruct.NVIC_IRQChannel = EXTI9_5_IRQn;

NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;

NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 1;

NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0;

NVIC_Init(&NVIC_InitStruct);

 

}

/**

  * @brief  中断服务函数，采用4倍频测速

  * @param  None

  * @retval None 

  */

int Encoder_EXTI=0;

void EXTI9_5_IRQHandler(void)

{

 

if(EXTI_GetITStatus(EXTI_Line6) != RESET)//右轮A相 PB6

{

EXTI_ClearITPendingBit(EXTI_Line6);  //清除LINE上的中断标志位

if(PBin(6)==0)   //这里判断检测到的是否是下降沿

{

if(PBin(7)==0)   Encoder_EXTI++;//B相的电平如果是低，电机就是正转加1

else             Encoder_EXTI--;//否则就是反转减1

}

else                  //上升沿

{ 

if(PBin(7)==1)  Encoder_EXTI++; //B相电平如果为高，电机就是正转加1

else             Encoder_EXTI--;//否则就是反转减1

}

}

 

if(EXTI_GetITStatus(EXTI_Line7) != RESET)//右轮B相 PB7

{

EXTI_ClearITPendingBit(EXTI_Line7);  //清除LINE上的中断标志位

if(PBin(7)==0)   //这里判断检测到的是否是下降沿

{

if(PBin(6)==1)   Encoder_EXTI++;//A相的电平如果是高，电机就是正转加1

else             Encoder_EXTI--;//否则就是反转减1

}

else                  //上升沿

{ 

if(PBin(6)==0)  Encoder_EXTI++; //A相电平如果为低，电机就是正转加1

else             Encoder_EXTI--;//否则就是反转减1

}

}

 

}

/**

  * @brief  获取中断的计数值

  * @param  None

  * @retval None 

  */

int16_t Encoder_Get(void)

{

int16_t Temp;

Temp = Encoder_EXTI;

Encoder_EXTI =0;

return Temp;

}

 /**

  * @brief  定时器中断，每100ms更新一次速度

  * @param  None

  * @retval None 

  */

  void TIM2_IRQHandler(void)

{

if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)

{

Speed = Encoder_Get();

TIM_ClearITPendingBit(TIM2, TIM_IT_Update);

}

}

 

 

 

Calculation of the actual speed of the motor wheel:

 

1. First of all, the essence of the speed calculation formula is:

 

in=s/t

 

t: known encoder counting cycle (100ms), s: the distance traveled by the wheel in this counting cycle.

 

2. The next step is to calculate s.

 

We know that the encoder collects the number of pulses per unit time, and the encoder's code disk is installed at the end of the motor shaft, measuring the speed of the motor output shaft. The motor shaft also needs to pass through a reducer before it can be connected to the wheel.

Therefore, we can first calculate the distance traveled by a pulse wheel per unit time.

 

l=2∗PI∗R/(4∗n∗i)

 

2 * PI *R is the circumference of the wheel, R is the radius of the wheel (34mm), using 4 times the frequency counting, n is the number of pulses of the encoder's code disk in one rotation (11), and i is the motor's reduction ratio (30).

 

3. The distance traveled by a pulse wheel can be directly calculated by hand, so we multiply it by the number of pulses measured by the encoder to know the distance s traveled by the wheel in this counting cycle.

Therefore,

 

s=m∗l

 

m is the number of pulses measured by the encoder.

The above calculation process is expressed in C code as follows:

 

/**

  * @brief  编码器读数转换为轮子速度（mm/s）

  * @param encoder：编码器计数

  * @retval : Velocity 轮子速度

  */

int Get_Velocity_Form_Encoder(int encoder)

{

float Distance,Velocity;

Distance= 2*3.14159*34/(4*11*30);//单位是mm

Velocity= encoder*Distance/0.1; //单位是mm/s。0.1就是编码器计数周期100ms，0.1s

 

return Velocity;

}

 

 

In motor control, we output a PWM duty cycle value to the motor.

The basic formula of position PID:

 

The control flow chart is as follows:

 

In the above figure, we can generally change the target value by programming through buttons or switches. The measured position is to collect the encoder data through stm32. The difference between the target position and the measured position is the current system deviation. It is sent to the PID controller for calculation and output, and then the motor drive power amplifier controls the rotation of the motor to reduce the deviation and finally reach the target position.

 

 

The code for position PID implemented in C language is as follows:

First, define a PID parameter structure :

 

typedef struct

{

float target_val;   //目标值

float Error;          /*第 k 次偏差 */

float LastError;     /* Error[-1],第 k-1 次偏差 */

float PrevError;    /* Error[-2],第 k-2 次偏差 */

float Kp,Ki,Kd;     //比例、积分、微分系数

float integral;     //积分值

float output_val;   //输出值

}PID;

 

Then make a function to initialize the PID parameters:

 

/**

  * @brief  PID参数初始化

  * @note 无

  * @retval 无

  */

void PID_param_init()

{

PosionPID.target_val=3600;

PosionPID.output_val=0.0;

PosionPID.Error=0.0;

PosionPID.LastError=0.0;

PosionPID.integral=0.0;

PosionPID.Kp = 10;

PosionPID.Ki = 0.5;

PosionPID.Kd = 0.8;

 

 

}

 

 

Finally, write the implementation function of the position PID according to the formula:

 

/**

  * @brief  位置PID算法实现

  * @param  actual_val:实际测量值

  * @note 无

  * @retval 通过PID计算后的输出

  */

float PosionPID_realize(PID *pid, float actual_val)

{

/*计算目标值与实际值的误差*/

pid->Error = pid->target_val - actual_val;

/*积分项*/

pid->integral += pid->Error;

/*PID算法实现*/

pid->output_val = pid->Kp * pid->Error +

                  pid->Ki * pid->integral +

                  pid->Kd *(pid->Error -pid->LastError);

/*误差传递*/

pid-> LastError = pid->Error;

/*返回当前实际值*/

return pid->output_val;

}

 

 

The function input parameters are the encoder's speed measurement value and the structure of PID parameters, and the return value is the motor control PWM.

 

 

 

Incremental PID is also called speed loop PID. Speed ​​closed-loop control is to measure the speed information of the motor according to the number of pulses obtained per unit time (here the M method is used for speed measurement), and compare it with the target value to obtain the control deviation, and then control the deviation by proportion, integration, and differentiation to make the deviation tend to zero.

 

In our speed control closed-loop system, only PI control is used, so the PID controller can be simplified to the following formula:

 

The control block diagram is the same as the position type.

 

In the above figure, we can generally program the target speed by pressing buttons or switches to change the target value. The speed measurement has been mentioned in the encoder section before, which is to collect the encoder data through the microcontroller at regular intervals and clear it . The difference between the target speed and the measured speed is the deviation of the current system. It is sent to the PID controller for calculation and output, and then the motor drive power amplifier controls the rotation of the motor to reduce the deviation and finally reach the target speed.

 

 

The structure definition and member initialization of the incremental PID are the same as those of the positional PID. The code implemented in C language is as follows:

 

/**

  * @brief  速度PID算法实现

  * @param  actual_val:实际值

  * @note 无

  * @retval 通过PID计算后的输出

  */

float addPID_realize(PID *pid, float actual_val)

{

/*计算目标值与实际值的误差*/

pid->Error = pid->target_val - actual_val;

/*PID算法实现，照搬公式*/

pid->output_val += pid->Kp * (pid->Error - pid-> LastError) +

                  pid->Ki * pid->Error +

                  pid->Kd *(pid->Error -2*pid->LastError+pid->PrevError);

/*误差传递*/

pid-> PrevError = pid->LastError;

pid-> LastError = pid->Error;

/*返回当前实际值*/

return pid->output_val;

}

 

 

The function input parameters are the encoder's speed measurement value and the structure of PID parameters. The return value is the motor control PWM. It can be seen that the incremental PID is only related to the last three measurement values.

 

 

Cascade PID means first inputting the position PID, then passing through the speed PID, and finally outputting it.

 

 

The performance indicators of automatic control systems mainly include three aspects: stability, rapidity, and accuracy.

 

Stability : After the system is subjected to external action, if the control system makes its controlled variable consistent with the given expected value as time goes by, the system is called stable, which we generally call system convergence. If the controlled variable deviates more and more from the given value as time goes by, the system is called unstable, which we generally call system divergence. Only a stable system can complete the task of automatic control, so system stability is a necessary condition to ensure the normal operation of the control system. The initial deviation of the controlled variable from the given value of a stable control system should gradually decrease and tend to zero as time goes by.

 

Rapidity : Rapidity refers to the length of time the dynamic process of the system takes. The shorter the process time, the better the system rapidity. The longer the process time lasts, the slower the system response is and the more difficult it is to achieve a rapidly changing command signal. Stability and rapidity reflect the performance of the system in the control process. During the tracking process of the system, the smaller the deviation of the controlled variable from the given value and the shorter the deviation time, the higher the dynamic accuracy of the system.

 

Accuracy : It refers to the deviation of the controlled variable (or feedback) from the given value after the dynamic process of the system ends. This deviation is the steady-state error. It is an indicator to measure the steady-state accuracy of the system and reflects the performance in the later stage of the dynamic process.

 

In practical production projects, different control systems have different requirements for controller effects. For example, balancing cars and inverted pendulums have very high requirements for system speed, and too slow response will cause the system to lose control. The automatic door and window opening and closing system in smart homes does not require high speed, but has high requirements for stability and accuracy, so it is necessary to strictly control the overshoot and static error of the system.

 

 

This article mainly introduces the position PID and incremental PID commonly used in the PID control of motors.