You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
122 lines
2.7 KiB
122 lines
2.7 KiB
#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); |
|
} |
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|