MotorPid/stm32/HARDWARE/TIMER/timer.c

#include "timer.h"
#include "led.h"
#include "wifi.h"
#include "math.h"
#include "encoder.h"
#include "motor.h"

extern char smog_value_buf[7];  
extern char temp_value_buf[7];
extern char beam_value_buf[7]; 

extern struct UserInfo userInfo;

/**
 * @description: 设置pwm
 * @param {int} motor
 * @return {*}
 */
void Set_Pwm(int motor)
{
	if(motor<0)	AIN2=1,	AIN1=0;
	else AIN2=0,AIN1=1;
	PWMA=abs(motor);
}


/**
 * @description: 卡尔曼滤波器
 * @param {unsigned long} inData
 * @return {*}
 */
int KalmanFilter(int inData)
{
    static float  kalman = 0; //上次卡尔曼值(估计出的最优值)
    static float  p = 10;
    float  q = 0.02; //q:过程噪声
    float  r = 7.0000; //r:测量噪声
    float  kg = 0; //kg:卡尔曼增益
    p += q;
    kg = p / ( p + r ); //计算卡尔曼增益
    kalman = kalman + (kg * (inData - kalman)); //计算本次滤波估计值
    p = (1 - kg) * p; //更新测量方差
    return (int)kalman; //返回估计值
}

/**
 * @description: 
 * @param {int} Encoder  编码器速度
 * @param {int} Target   目标数据
 * @return {*}
 */
int velocity_PID(int Encoder,int Target){
	static float velocity_KP = 7.7, velocity_KI = 1.1; 
	static int Last_bias;
	static int Bias, pwm;
	Bias = Encoder - Target;  //计算偏差
	pwm += velocity_KP * Bias  + velocity_KI * (Bias - Last_bias); //增量式PI控制器
	if(pwm > 7200) pwm = 7200;
	if(pwm < -7200) pwm = -7200;
	Last_bias = Bias; //保存上一次偏差 
	return pwm;
}


/**
 * @description: 
 * @param {u16} arr
 * @param {u16} psc
 * @return {*}
 */
void TIM3_Int_Init(u16 arr,u16 psc)
{
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	NVIC_InitTypeDef NVIC_InitStructure;

	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); 

	TIM_TimeBaseStructure.TIM_Period = arr; 
	TIM_TimeBaseStructure.TIM_Prescaler =psc; 
	TIM_TimeBaseStructure.TIM_ClockDivision = 0; 
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; 
	TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
 
	TIM_ITConfig(  
		TIM3, 
		TIM_IT_Update ,
		ENABLE  
		);
	NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;  
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;  
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;  
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 
	NVIC_Init(&NVIC_InitStructure);  

	TIM_Cmd(TIM3, ENABLE);  				 
}

void TIM3_IRQHandler(void)   
{
	int pwm = 0;
	if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) 
	{
		userInfo.currentSpeed = Read_Encoder(2);
		userInfo.currentSpeed = KalmanFilter(userInfo.currentSpeed);
		if(userInfo.run)pwm = velocity_PID(userInfo.currentSpeed, userInfo.targetSpeed);
		else pwm = 0;
		userInfo.pwm_val = pwm;
		Set_Pwm(pwm);
		TIM_ClearITPendingBit(TIM3, TIM_IT_Update); 
	}
}