[untitled] PID control TT encoder motor

Take notes of my study

Begin to learn PID It's hard for him to , It's not . First, we need to understand PID The basic process of controlling the motor , Then match it with your theoretical knowledge .

Let's talk about the specific implementation , First, set the timer of MCU to read AB Number of pulses in phase （ The first thing to note is that the channels for acquiring pulses can only be the first two channels of the timer ）, And then in PID Compare the number of pulses you set with the measured number of pulses . Generally speaking, we set the speed , So we need to convert the speed into the number of pulses . for instance ：TT The motor 13 The of the line makes the frequency quadruple , There are 2496 Pulse , We set 50 Turn every minute （ There's only one wheel speed here , If it is the internal speed of the motor, it needs to be divided by your reduction ratio ）, Then we set the timer 10ms Read value once . that 10ms The number of pulses should be 50*2496/6000（ One minute has 6000 individual 10ms）, Therefore, the set pulse and the currently measured pulse are brought into PID Calculate the value according to the calculation formula , In the abstract, it is a basis deviation , The control effect . Generate a by calculating the value PWM The signal .

Here is part of the code , You can have a look at , Basically, that's all .

1. Set the timer

#include "encoder.h"
#include "delay.h"
#include "usart.h"


//PA0
//PA1
void TIM5_Encoder_Init(void)
{
    
	GPIO_InitTypeDef GPIO_InitStructure; 
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_ICInitTypeDef TIM_ICInitStructure;

	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);	
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);	
	
	GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_0 | GPIO_Pin_1;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;    
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);

	TIM_TimeBaseStructure.TIM_Prescaler = 0x00;  // No prescaling 
	TIM_TimeBaseStructure.TIM_Period = 65535;  
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;   
	TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);
	
	TIM_EncoderInterfaceConfig(TIM5, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
	TIM_ICStructInit(&TIM_ICInitStructure);
	TIM_ICInitStructure.TIM_ICFilter = 10;
	TIM_ICInit(TIM5, &TIM_ICInitStructure);

	TIM_SetCounter(TIM5,0);
	TIM_Cmd(TIM5, ENABLE);    
}

2.PID Calculation part

There are two calculation formulas here, which are written differently , You can use either ,PID The calculation formula uses a lot to see your own needs .

#include "pid.h"


static double Kp=10;
static double Ki=0;
static double Kd=0;
int shouxie_PID_ZLP(int now , int set)
{
    
  int PWM,Bias,Bias_D;
	static int error,last_Bias;
	Bias = now - set;
	error +=Bias;
	Bias_D=Bias-last_Bias;
	PWM = Kp*Bias + Ki*error +Kd*Bias_D;
	last_Bias = Bias;
	return PWM;

}
static double   ProportionA=70;                               
static double   IntegralA=25;                                
static double   DerivativeA=35;

int SPEED_PID_A(int now,int set)
{
    
	static int  LastError;	//Error[-1]
	static int  PrevError;  //Error[-2]
    int iError,Output;     
	iError=set-now;
	Output = ((ProportionA * (iError - LastError)) + (IntegralA * iError) + (DerivativeA * (iError - 2*LastError + PrevError)));	
  PrevError=LastError;                    
  LastError=iError;
  return Output;                              
}

3. Control the output

#include "control.h"
#include "pid.h"
#include "datascope.h"
#include "motor.h"
#include "usart.h"

 
int para_A;
int Motor_A;
int SetRPMA=100;
#define SetPointA SetRPMA*2496/6000


int myabs(int a){
    
	int temp;
	if(a < 0) temp = -a;
	else temp = a;
	return temp;
}
int Read_Encoder(u8 x)
{
    
   int Encoder_TIM;    
   switch(x)
	 {
    
		 case 5:  Encoder_TIM= (short)TIM5 -> CNT;  TIM5 -> CNT=0;break;
	
		 default: 	Encoder_TIM=0;
	 }
		return Encoder_TIM;
}


int Encoder_A;  
int Send_Count,i;
extern int adc_data;
int position_out;
int ac_speed_b;
int bigout_B;

void TIM6_IRQHandler(void)
{
    

	if(TIM_GetFlagStatus(TIM6, TIM_IT_Update) != RESET) 
	{
    

		
		Encoder_A=Read_Encoder(5);  // Read the value of the encoder  
		
		para_A=SPEED_PID_A(Encoder_A,SetPointA);
		/*  Bring the set pulse and measured pulse into PID The controller  */	
		
        printf("%d\r\n",Encoder_A);
		  
		

		if((para_A<-3)||(para_A>3)) 
		{
    
			Motor_A +=para_A;  
		} 
		if(Motor_A>7000)Motor_A=7000;// Limiting 
		if(Motor_A<-7000)Motor_A=-7000;
		
		if(Motor_A > 0){
    
		MotorA1 = 0;
		MotorA2 = 1;
		}else{
    
		MotorA1 = 1;
		MotorA2 = 0;
		}TIM8->CCR1 = myabs(Motor_A);
		/* Timer 8  I use a counting cycle of 10000 ,CCR1 The value of is  myabs(Motor_A)*/
	}
	TIM_ClearITPendingBit(TIM6, TIM_FLAG_Update);   

}

If you don't understand or I'm wrong, you can write it in the comment area .