STM32自平衡小车源代码 - - Powered by Discuz!

#include "stm32f10x.h"
#include "MPU6050.h"
#include "motor.h"
#include "control.h"
#include <math.h>
//******角度参数************
float Gyro_y; //Y轴陀螺仪数据暂存
float Angle_gy; //由角速度计算的倾斜角度
float Accel_x; //X轴加速度值暂存
float Angle_ax; //由加速度计算的倾斜角度
float Angle; //小车最终倾斜角度
//******PWM参数*************
int speed_mr; //右电机转速
int speed_ml; //左电机转速
float PWM; //综合PWM计算
//******电机参数*************
float speed_r_l; //电机转速
float speed; //电机转速滤波
//******卡尔曼参数************
const float Q_angle=0.001;
const float Q_gyro=0.003;
const float R_angle=0.5;
const float dt=0.01; //dt为kalman滤波器采样时间;
const char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
static const float Kp = 35.0; //PID参数
static const float Kd = 2.5; //PID参数
static const float Ksp = 10.0; //PID参数
void TIM4_Configuration(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;//定义一个定时器结构体变量
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);//使能定时器4
TIM_TimeBaseStructure.TIM_Period = (10000 - 1); //计数10000次，因为从0开始，所以减1
TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1); //时钟72分频，因为0不分频，所以减1
TIM_TimeBaseStructure.TIM_ClockDivision = 0; // 使用的采样频率之间的分频比例
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //向上计数
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); //初始化定时器4
TIM_ClearITPendingBit(TIM4, TIM_IT_Update); //清除定时器4中断标志位
TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE); //打开定时器4中断
TIM_Cmd(TIM4, ENABLE); //计数器使能，开始计数
}
void NVIC_Configuration(void)
{
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0000);
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
/********************注册定时器4中断处理函数***********************/
NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
/******************注册串口1中断服务函数************************/
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//配置串口中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;//占先式优先级设置为0
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; //副优先级设置为0
NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//中断禁止
NVIC_Init(&NVIC_InitStructure);//中断初始化
}
void Control_Init(void)
{
TIM4_Configuration();
NVIC_Configuration();
}
/****************定时器4中断服务函数***************************************/
void TIM4_IRQHandler(void)
{
if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
Angle_Calculate();// 车体倾角计算
Speed_Calculate();//车速计算
PWM_Calculate();//电机PWM值计算
Motor_Control(PWM);//调节电机参数
}
}
/*****************卡尔曼滤波**************************************************/
void Kalman_Filter(float Accel,float Gyro)
{
Angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]= -PP[1][1];
Pdot[2]= -PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle; //zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}
void Angle_Calculate(void)
{
static uint8_t DataBuffer[2]; //数据缓存
/****************************加速度****************************************/
I2C_ReadBuffer(DataBuffer, ACCEL_XOUT_H, 2);
Accel_x = (short)((DataBuffer[0]<<8)+DataBuffer[1]); //读取X轴加速度
Angle_ax = (Accel_x +220) /16384; //去除零点偏移,计算得到角度（弧度）
Angle_ax = Angle_ax*1.2*180/3.14; //弧度转换为度,
/****************************角速度****************************************/
I2C_ReadBuffer(DataBuffer, GYRO_YOUT_H, 2);
Gyro_y = (short)((DataBuffer[0]<<8)+DataBuffer[1]); //静止时角速度Y轴输出为-18左右
Gyro_y = (Gyro_y + 18)/16.4; //去除零点偏移，计算角速度值
//Angle_gy = Angle_gy + Gyro_y*0.01; //角速度积分得到倾斜角度,因为卡尔曼计算带有时间dt，所以此处不用积分
/***************************卡尔曼融合*************************************/
Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角
/****************************互补滤波****************************************/
/***补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与
****陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的
****角度 0.5为放大倍数，可调节补偿度；0.01为系统周期10ms
*************************************************************/
// Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01); //因为加或不加此语句处理效果一样，故省略，原因搞不清楚。。。
}
void Speed_Calculate(void)
{
speed_mr=TIM_GetCounter(TIM2)-0x7fff;
speed_ml=TIM_GetCounter(TIM3)-0x7fff; //读取编码器寄存器计数值,并减去中间值,得到速度矢量
TIM_SetCounter(TIM2, 0x7fff);
TIM_SetCounter(TIM3, 0x7fff); //重置编码器计数值
speed_r_l =(speed_mr + speed_ml)*0.5;
speed *= 0.7; //车轮速度滤波
speed += speed_r_l*0.3;
}
void PWM_Calculate(void)
{
if(Angle<-40||Angle>40) //倾角过大就关闭电机，进入死循环，直到复位
{
Motor_Stop();
while(1);
}
PWM = (short)(Kp*Angle + Kd*Gyro_y); //PID：角速度和角度调节
PWM += Ksp*speed; //PID：车速度调节
}

复制代码