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.
123 lines
2.7 KiB
123 lines
2.7 KiB
3 years ago
|
#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);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|