代码阅读，学习mpu6050滤波"/>
平衡车代码阅读，学习mpu6050滤波
mpu6050.c
#include "MPU6050.h"
#include "IOI2C.h"
#include "usart.h"/**************************************************************************
作者：平衡小车之家
我的淘宝小店：/
**************************************************************************/
#define PRINT_ACCEL (0x01)
#define PRINT_GYRO (0x02)
#define PRINT_QUAT (0x04)
#define ACCEL_ON (0x01)
#define GYRO_ON (0x02)
#define MOTION (0)
#define NO_MOTION (1)
#define DEFAULT_MPU_HZ (200)
#define FLASH_SIZE (512)
#define FLASH_MEM_START ((void*)0x1800)
#define q30 1073741824.0f
short gyro[3], accel[3], sensors;
float Pitch,Roll;
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
static signed char gyro_orientation[9] = {-1, 0, 0,0,-1, 0,0, 0, 1};static unsigned short inv_row_2_scale(const signed char *row)
{unsigned short b;if (row[0] > 0)b = 0;else if (row[0] < 0)b = 4;else if (row[1] > 0)b = 1;else if (row[1] < 0)b = 5;else if (row[2] > 0)b = 2;else if (row[2] < 0)b = 6;elseb = 7; // errorreturn b;
}static unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
{unsigned short scalar;scalar = inv_row_2_scale(mtx);scalar |= inv_row_2_scale(mtx + 3) << 3;scalar |= inv_row_2_scale(mtx + 6) << 6;return scalar;
}static void run_self_test(void)
{int result;long gyro[3], accel[3];result = mpu_run_self_test(gyro, accel);if (result == 0x7) {/* Test passed. We can trust the gyro data here, so let's push it down* to the DMP.*/float sens;unsigned short accel_sens;mpu_get_gyro_sens(&sens);gyro[0] = (long)(gyro[0] * sens);gyro[1] = (long)(gyro[1] * sens);gyro[2] = (long)(gyro[2] * sens);dmp_set_gyro_bias(gyro);mpu_get_accel_sens(&accel_sens);accel[0] *= accel_sens;accel[1] *= accel_sens;accel[2] *= accel_sens;dmp_set_accel_bias(accel);printf("setting bias succesfully ......\r\n");}
}uint8_t buffer[14];int16_t MPU6050_FIFO[6][11];
int16_t Gx_offset=0,Gy_offset=0,Gz_offset=0;/**************************实现函数********************************************
*函数原型: void MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
*功 能: 将新的ADC数据更新到 FIFO数组，进行滤波处理
*******************************************************************************/void MPU6050_newValues(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz)
{
unsigned char i ;
int32_t sum=0;
for(i=1;i<10;i++){ //FIFO 操作
MPU6050_FIFO[0][i-1]=MPU6050_FIFO[0][i];
MPU6050_FIFO[1][i-1]=MPU6050_FIFO[1][i];
MPU6050_FIFO[2][i-1]=MPU6050_FIFO[2][i];
MPU6050_FIFO[3][i-1]=MPU6050_FIFO[3][i];
MPU6050_FIFO[4][i-1]=MPU6050_FIFO[4][i];
MPU6050_FIFO[5][i-1]=MPU6050_FIFO[5][i];
}
MPU6050_FIFO[0][9]=ax;//将新的数据放置到 数据的最后面
MPU6050_FIFO[1][9]=ay;
MPU6050_FIFO[2][9]=az;
MPU6050_FIFO[3][9]=gx;
MPU6050_FIFO[4][9]=gy;
MPU6050_FIFO[5][9]=gz;sum=0;
for(i=0;i<10;i++){ //求当前数组的合，再取平均值sum+=MPU6050_FIFO[0][i];
}
MPU6050_FIFO[0][10]=sum/10;sum=0;
for(i=0;i<10;i++){sum+=MPU6050_FIFO[1][i];
}
MPU6050_FIFO[1][10]=sum/10;sum=0;
for(i=0;i<10;i++){sum+=MPU6050_FIFO[2][i];
}
MPU6050_FIFO[2][10]=sum/10;sum=0;
for(i=0;i<10;i++){sum+=MPU6050_FIFO[3][i];
}
MPU6050_FIFO[3][10]=sum/10;sum=0;
for(i=0;i<10;i++){sum+=MPU6050_FIFO[4][i];
}
MPU6050_FIFO[4][10]=sum/10;sum=0;
for(i=0;i<10;i++){sum+=MPU6050_FIFO[5][i];
}
MPU6050_FIFO[5][10]=sum/10;
}/**************************实现函数********************************************
*函数原型: void MPU6050_setClockSource(uint8_t source)
*功 能: 设置 MPU6050 的时钟源* CLK_SEL | Clock Source* --------+--------------------------------------* 0 | Internal oscillator* 1 | PLL with X Gyro reference* 2 | PLL with Y Gyro reference* 3 | PLL with Z Gyro reference* 4 | PLL with external 32.768kHz reference* 5 | PLL with external 19.2MHz reference* 6 | Reserved* 7 | Stops the clock and keeps the timing generator in reset
*******************************************************************************/
void MPU6050_setClockSource(uint8_t source){IICwriteBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);}/** Set full-scale gyroscope range.* @param range New full-scale gyroscope range value* @see getFullScaleRange()* @see MPU6050_GYRO_FS_250* @see MPU6050_RA_GYRO_CONFIG* @see MPU6050_GCONFIG_FS_SEL_BIT* @see MPU6050_GCONFIG_FS_SEL_LENGTH*/
void MPU6050_setFullScaleGyroRange(uint8_t range) {IICwriteBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
}/**************************实现函数********************************************
*函数原型: void MPU6050_setFullScaleAccelRange(uint8_t range)
*功 能: 设置 MPU6050 加速度计的最大量程
*******************************************************************************/
void MPU6050_setFullScaleAccelRange(uint8_t range) {IICwriteBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
}/**************************实现函数********************************************
*函数原型: void MPU6050_setSleepEnabled(uint8_t enabled)
*功 能: 设置 MPU6050 是否进入睡眠模式enabled =1 睡觉enabled =0 工作
*******************************************************************************/
void MPU6050_setSleepEnabled(uint8_t enabled) {IICwriteBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}/**************************实现函数********************************************
*函数原型: uint8_t MPU6050_getDeviceID(void)
*功 能: 读取 MPU6050 WHO_AM_I 标识 将返回 0x68
*******************************************************************************/
uint8_t MPU6050_getDeviceID(void) {IICreadBytes(devAddr, MPU6050_RA_WHO_AM_I, 1, buffer);return buffer[0];
}/**************************实现函数********************************************
*函数原型: uint8_t MPU6050_testConnection(void)
*功 能: 检测MPU6050 是否已经连接
*******************************************************************************/
uint8_t MPU6050_testConnection(void) {if(MPU6050_getDeviceID() == 0x68) //0b01101000;return 1;else return 0;
}/**************************实现函数********************************************
*函数原型: void MPU6050_setI2CMasterModeEnabled(uint8_t enabled)
*功 能: 设置 MPU6050 是否为AUX I2C线的主机
*******************************************************************************/
void MPU6050_setI2CMasterModeEnabled(uint8_t enabled) {IICwriteBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
}/**************************实现函数********************************************
*函数原型: void MPU6050_setI2CBypassEnabled(uint8_t enabled)
*功 能: 设置 MPU6050 是否为AUX I2C线的主机
*******************************************************************************/
void MPU6050_setI2CBypassEnabled(uint8_t enabled) {IICwriteBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
}/**************************实现函数********************************************
*函数原型: void MPU6050_initialize(void)
*功 能: 初始化 MPU6050 以进入可用状态。
*******************************************************************************/
void MPU6050_initialize(void) {MPU6050_setClockSource(MPU6050_CLOCK_PLL_YGYRO); //设置时钟MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//陀螺仪最大量程 +-1000度每秒MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2); //加速度度最大量程 +-2GMPU6050_setSleepEnabled(0); //进入工作状态MPU6050_setI2CMasterModeEnabled(0); //不让MPU6050 控制AUXI2CMPU6050_setI2CBypassEnabled(0); //主控制器的I2C与 MPU6050的AUXI2C 直通。控制器可以直接访问HMC5883L
}/**************************************************************************
函数功能：MPU6050内置DMP的初始化
入口参数：无
返回 值：无
作 者：平衡小车之家
**************************************************************************/
void DMP_Init(void)
{ u8 temp[1]={0};i2cRead(0x68,0x75,1,temp);Flag_Show=1;printf("mpu_set_sensor complete ......\r\n");if(temp[0]!=0x68)NVIC_SystemReset();if(!mpu_init()){if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))printf("mpu_set_sensor complete ......\r\n");if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))printf("mpu_configure_fifo complete ......\r\n");if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))printf("mpu_set_sample_rate complete ......\r\n");if(!dmp_load_motion_driver_firmware())printf("dmp_load_motion_driver_firmware complete ......\r\n");if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))printf("dmp_set_orientation complete ......\r\n");if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |DMP_FEATURE_GYRO_CAL))printf("dmp_enable_feature complete ......\r\n");if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))printf("dmp_set_fifo_rate complete ......\r\n");run_self_test();if(!mpu_set_dmp_state(1))printf("mpu_set_dmp_state complete ......\r\n");}Flag_Show=0;
}
/**************************************************************************
函数功能：读取MPU6050内置DMP的姿态信息
入口参数：无
返回 值：无
作 者：平衡小车之家
**************************************************************************/
void Read_DMP(void)
{ unsigned long sensor_timestamp;unsigned char more;long quat[4];dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); if (sensors & INV_WXYZ_QUAT ){ q0=quat[0] / q30;q1=quat[1] / q30;q2=quat[2] / q30;q3=quat[3] / q30;Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll}}
/**************************************************************************
函数功能：读取MPU6050内置温度传感器数据
入口参数：无
返回 值：摄氏温度
作 者：平衡小车之家
**************************************************************************/
int Read_Temperature(void)
{ float Temp;Temp=(I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_TEMP_OUT_L);if(Temp>32768) Temp-=65536;Temp=(36.53+Temp/340)*10;return (int)Temp;
}
//------------------End of File----------------------------
control.c
#include "control.h"
#include "filter.h" /**************************************************************************
作者：平衡小车之家
我的淘宝小店：/
**************************************************************************/
int Balance_Pwm,Velocity_Pwm,Turn_Pwm;
u8 Flag_Target;
int Voltage_Temp,Voltage_Count,Voltage_All;
/**************************************************************************
函数功能：所有的控制代码都在这里面5ms定时中断由MPU6050的INT引脚触发严格保证采样和数据处理的时间同步
**************************************************************************/
int EXTI9_5_IRQHandler(void)
{ if(PBin(5)==0) { EXTI->PR=1<<5; //清除LINE5上的中断标志位 Flag_Target=!Flag_Target;if(delay_flag==1){if(++delay_50==10) delay_50=0,delay_flag=0; //给主函数提供精准延时}if(Flag_Target==1) //5ms读取一次陀螺仪和加速度计的值，更高的采样频率可以改善卡尔曼滤波和互补滤波的效果{Get_Angle(Way_Angle); //===更新姿态 Voltage_Temp=Get_battery_volt(); //=====读取电池电压 Voltage_Count++; //=====平均值计数器Voltage_All+=Voltage_Temp; //=====多次采样累积if(Voltage_Count==100) Voltage=Voltage_All/100,Voltage_All=0,Voltage_Count=0;//=====求平均值 return 0; } //10ms控制一次，为了保证M法测速的时间基准，首先读取编码器数据Encoder_Left=-Read_Encoder(2); //===读取编码器的值，因为两个电机的旋转了180度的，所以对其中一个取反，保证输出极性一致Encoder_Right=Read_Encoder(4); //===读取编码器的值Get_Angle(Way_Angle); //===更新姿态 Read_Distane(); //读取超声波测量得到的距离if(Bi_zhang==0)Led_Flash(100); //===LED闪烁;常规模式 1s改变一次指示灯的状态 if(Bi_zhang==1)Led_Flash(0); //===LED闪烁;避障模式 指示灯常亮 Key(); //===扫描按键状态 单击双击可以改变小车运行状态Balance_Pwm =balance(Angle_Balance,Gyro_Balance); //===平衡PID控制 Velocity_Pwm=velocity(Encoder_Left,Encoder_Right); //===速度环PID控制 记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点Turn_Pwm =turn(Encoder_Left,Encoder_Right,Gyro_Turn); //===转向环PID控制 Moto1=Balance_Pwm+Velocity_Pwm-Turn_Pwm; //===计算左轮电机最终PWMMoto2=Balance_Pwm+Velocity_Pwm+Turn_Pwm; //===计算右轮电机最终PWMXianfu_Pwm(); //===PWM限幅if(Pick_Up(Acceleration_Z,Angle_Balance,Encoder_Left,Encoder_Right))//===检查是否小车被那起Flag_Stop=1; //===如果被拿起就关闭电机if(Put_Down(Angle_Balance,Encoder_Left,Encoder_Right)) //===检查是否小车被放下Flag_Stop=0; //===如果被放下就启动电机if(Turn_Off(Angle_Balance,Voltage)==0) //===如果不存在异常Set_Pwm(Moto1,Moto2); //===赋值给PWM寄存器 } return 0;
} /**************************************************************************
函数功能：直立PD控制
入口参数：角度、角速度
返回 值：直立控制PWM
作 者：平衡小车之家
**************************************************************************/
int balance(float Angle,float Gyro)
{ float Bias;int balance;Bias=Angle-ZHONGZHI; //===求出平衡的角度中值 和机械相关balance=Balance_Kp*Bias+Gyro*Balance_Kd; //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数 return balance;
}/**************************************************************************
函数功能：速度PI控制 修改前进后退遥控速度，请修Target_Velocity，比如，改成60就比较慢了
入口参数：左轮编码器、右轮编码器
返回 值：速度控制PWM
作 者：平衡小车之家
**************************************************************************/
int velocity(int encoder_left,int encoder_right)
{ static float Velocity,Encoder_Least,Encoder,Movement;static float Encoder_Integral,Target_Velocity=90;//=============遥控前进后退部分=======================// if(1==Flag_Qian) Movement=-Target_Velocity; //===前进标志位置1 else if(1==Flag_Hou) Movement=Target_Velocity; //===后退标志位置1else Movement=0; if(Bi_zhang==1&&Distance<500&&Flag_Left!=1&&Flag_Right!=1) //避障标志位置1且非遥控转弯的时候，进入避障模式Movement=Target_Velocity;//=============速度PI控制器=======================// Encoder_Least =(Encoder_Left+Encoder_Right)-0; //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零） Encoder *= 0.7; //===一阶低通滤波器 Encoder += Encoder_Least*0.3; //===一阶低通滤波器 Encoder_Integral +=Encoder; //===积分出位移 积分时间：10msEncoder_Integral=Encoder_Integral-Movement; //===接收遥控器数据，控制前进后退if(Encoder_Integral>11000) Encoder_Integral=11000; //===积分限幅if(Encoder_Integral<-11000) Encoder_Integral=-11000; //===积分限幅 Velocity=Encoder*Velocity_Kp+Encoder_Integral*Velocity_Ki; //===速度控制 if(Turn_Off(Angle_Balance,Voltage)==1||Flag_Stop==1) Encoder_Integral=0; //===电机关闭后清除积分return Velocity;
}/**************************************************************************
函数功能：转向控制 修改转向速度，请修改Turn_Amplitude即可
入口参数：左轮编码器、右轮编码器、Z轴陀螺仪
返回 值：转向控制PWM
作 者：平衡小车之家
**************************************************************************/
int turn(int encoder_left,int encoder_right,float gyro)//转向控制
{static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.7,Turn_Count,Kp=42,Kd=0;float Turn_Amplitude=50/Flag_sudu; //=============遥控左右旋转部分=======================//if(1==Flag_Left||1==Flag_Right) //这一部分主要是根据旋转前的速度调整速度的起始速度，增加小车的适应性{if(++Turn_Count==1)Encoder_temp=myabs(encoder_left+encoder_right);Turn_Convert=50/Encoder_temp;if(Turn_Convert<0.4)Turn_Convert=0.4;if(Turn_Convert>1)Turn_Convert=1;} else{Turn_Convert=0.7;Turn_Count=0;Encoder_temp=0;} if(1==Flag_Left) Turn_Target-=Turn_Convert; //===接收转向遥控数据else if(1==Flag_Right) Turn_Target+=Turn_Convert; //===接收转向遥控数据else Turn_Target=0; //===接收转向遥控数据if(Turn_Target>Turn_Amplitude) Turn_Target=Turn_Amplitude; //===转向速度限幅if(Turn_Target<-Turn_Amplitude) Turn_Target=-Turn_Amplitude; //===转向速度限幅if(Flag_Qian==1||Flag_Hou==1) Kd=0.4; //===接收转向遥控数据直立行走的时候增加陀螺仪就纠正 else Kd=0.27; //=============转向PD控制器=======================//Turn=Turn_Target*Kp+gyro*Kd; //===结合Z轴陀螺仪进行PD控制return Turn;
}
/**************************************************************************
函数功能：赋值给PWM寄存器
入口参数：左轮PWM、右轮PWM
返回 值：无
**************************************************************************/
void Set_Pwm(int moto1,int moto2)
{int siqu=400;if(moto1>0) AIN2=1, AIN1=0;else AIN2=0, AIN1=1;PWMA=myabs(moto1)+siqu;if(moto2>0) BIN1=0, BIN2=1;else BIN1=1, BIN2=0;PWMB=myabs(moto2)+siqu;
}/**************************************************************************
函数功能：限制PWM赋值
入口参数：无
返回 值：无
**************************************************************************/
void Xianfu_Pwm(void)
{ int Amplitude=6900; //===PWM满幅是7200 限制在6900if(Flag_Qian==1) Moto1-=DIFFERENCE; //DIFFERENCE是一个衡量平衡小车电机和机械安装差异的一个变量。直接作用于输出，让小车具有更好的一致性。if(Flag_Hou==1) Moto2-=DIFFERENCE; //DIFFERENCE是一个衡量平衡小车电机和机械安装差异的一个变量。直接作用于输出，让小车具有更好的一致性。if(Moto1<-Amplitude) Moto1=-Amplitude; if(Moto1>Amplitude) Moto1=Amplitude; if(Moto2<-Amplitude) Moto2=-Amplitude; if(Moto2>Amplitude) Moto2=Amplitude; }
/**************************************************************************
函数功能：按键修改小车运行状态
入口参数：无
返回 值：无
**************************************************************************/
void Key(void)
{ u8 tmp,tmp2;tmp=click_N_Double(50); if(tmp==1)Flag_Stop=!Flag_Stop;//单击控制小车的启停if(tmp==2)Flag_Show=!Flag_Show;//双击控制小车的显示状态tmp2=Long_Press(); if(tmp2==1) Bi_zhang=!Bi_zhang; //长按控制小车是否进入超声波避障模式
}/**************************************************************************
函数功能：异常关闭电机
入口参数：倾角和电压
返回 值：1：异常 0：正常
**************************************************************************/
u8 Turn_Off(float angle, int voltage)
{u8 temp;if(angle<-40||angle>40||1==Flag_Stop||voltage<1110)//电池电压低于11.1V关闭电机{ //===倾角大于40度关闭电机temp=1; //===Flag_Stop置1关闭电机AIN1=0; AIN2=0;BIN1=0;BIN2=0;}elsetemp=0;return temp;
}/**************************************************************************
函数功能：获取角度 三种算法经过我们的调校，都非常理想
入口参数：获取角度的算法 1：DMP 2：卡尔曼 3：互补滤波
返回 值：无
**************************************************************************/
void Get_Angle(u8 way)
{ float Accel_Y,Accel_X,Accel_Z,Gyro_Y,Gyro_Z;Temperature=Read_Temperature(); //===读取MPU6050内置温度传感器数据，近似表示主板温度。 if(way==1) //===DMP的读取在数据采集中断提醒的时候，严格遵循时序要求{ Read_DMP(); //===读取加速度、角速度、倾角Angle_Balance=Pitch; //===更新平衡倾角Gyro_Balance=gyro[1]; //===更新平衡角速度Gyro_Turn=gyro[2]; //===更新转向角速度Acceleration_Z=accel[2]; //===更新Z轴加速度计} else{Gyro_Y=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_YOUT_L); //读取Y轴陀螺仪Gyro_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_GYRO_ZOUT_L); //读取Z轴陀螺仪Accel_X=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_XOUT_L); //读取X轴加速度计Accel_Z=(I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_H)<<8)+I2C_ReadOneByte(devAddr,MPU6050_RA_ACCEL_ZOUT_L); //读取Z轴加速度计if(Gyro_Y>32768) Gyro_Y-=65536; //数据类型转换 也可通过short强制类型转换if(Gyro_Z>32768) Gyro_Z-=65536; //数据类型转换if(Accel_X>32768) Accel_X-=65536; //数据类型转换if(Accel_Z>32768) Accel_Z-=65536; //数据类型转换Gyro_Balance=-Gyro_Y; //更新平衡角速度Accel_Y=atan2(Accel_X,Accel_Z)*180/PI; //计算倾角 Gyro_Y=Gyro_Y/16.4; //陀螺仪量程转换 if(Way_Angle==2) Kalman_Filter(Accel_Y,-Gyro_Y);//卡尔曼滤波 else if(Way_Angle==3) Yijielvbo(Accel_Y,-Gyro_Y); //互补滤波Angle_Balance=angle; //更新平衡倾角Gyro_Turn=Gyro_Z; //更新转向角速度Acceleration_Z=Accel_Z; //===更新Z轴加速度计 }
}
/**************************************************************************
函数功能：绝对值函数
入口参数：int
返回 值：unsigned int
**************************************************************************/
int myabs(int a)
{ int temp;if(a<0) temp=-a; else temp=a;return temp;
}
/**************************************************************************
函数功能：检测小车是否被拿起
入口参数：int
返回 值：unsigned int
**************************************************************************/
int Pick_Up(float Acceleration,float Angle,int encoder_left,int encoder_right)
{ static u16 flag,count0,count1,count2;if(flag==0) //第一步{if(myabs(encoder_left)+myabs(encoder_right)<30) //条件1，小车接近静止count0++;else count0=0; if(count0>10) flag=1,count0=0; } if(flag==1) //进入第二步{if(++count1>200) count1=0,flag=0; //超时不再等待2000msif(Acceleration>26000&&(Angle>(-20+ZHONGZHI))&&(Angle<(20+ZHONGZHI))) //条件2，小车是在0度附近被拿起flag=2; } if(flag==2) //第三步{if(++count2>100) count2=0,flag=0; //超时不再等待1000msif(myabs(encoder_left+encoder_right)>150) //条件3，小车的轮胎因为正反馈达到最大的转速 {flag=0; return 1; //检测到小车被拿起}}return 0;
}
/**************************************************************************
函数功能：检测小车是否被放下
入口参数：int
返回 值：unsigned int
**************************************************************************/
int Put_Down(float Angle,int encoder_left,int encoder_right)
{ static u16 flag,count; if(Flag_Stop==0) //防止误检 return 0; if(flag==0) {if(Angle>(-10+ZHONGZHI)&&Angle<(10+ZHONGZHI)&&encoder_left==0&&encoder_right==0) //条件1，小车是在0度附近的flag=1; } if(flag==1) {if(++count>50) //超时不再等待 500ms{count=0;flag=0;}if(encoder_left<-3&&encoder_right<-3&&encoder_left>-30&&encoder_right>-30) //条件2，小车的轮胎在未上电的时候被人为转动 {flag=0;flag=0;return 1; //检测到小车被放下}}return 0;
}filter.c
#include "filter.h"/**************************************************************************
作者：平衡小车之家
我的淘宝小店：/
**************************************************************************/
float K1 =0.02;
float angle, angle_dot;
float Q_angle=0.001;// 过程噪声的协方差
float Q_gyro=0.003;//0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
float R_angle=0.5;// 测量噪声的协方差 既测量偏差
float dt=0.005;//
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 } };/**************************************************************************
函数功能：简易卡尔曼滤波
入口参数：加速度、角速度
返回 值：无
**************************************************************************/
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; //后验估计angle_dot = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}/**************************************************************************
函数功能：一阶互补滤波
入口参数：加速度、角速度
返回 值：无
**************************************************************************/
void Yijielvbo(float angle_m, float gyro_m)
{angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * 0.005);
}
show.c
#include "show.h"/**************************************************************************
作者：平衡小车之家
我的淘宝小店：/
**************************************************************************/
unsigned char i; //计数变量
unsigned char Send_Count; //串口需要发送的数据个数
float Vol;
/**************************************************************************
函数功能：OLED显示
入口参数：无
返回 值：无
**************************************************************************/
void oled_show(void)
{//=============第一行显示小车模式=======================// if(Way_Angle==1) OLED_ShowString(0,0,"DMP");else if(Way_Angle==2) OLED_ShowString(0,0,"Kalman");else if(Way_Angle==3) OLED_ShowString(0,0,"Hubu");if(Bi_zhang==1) OLED_ShowString(60,0,"Bizhang");else OLED_ShowString(60,0,"Putong");//=============第二行显示温度和距离===============// OLED_ShowNumber(0,10,Temperature/10,2,12);OLED_ShowNumber(23,10,Temperature%10,1,12);OLED_ShowString(13,10,".");OLED_ShowString(35,10,"`C");OLED_ShowNumber(70,10,(u16)Distance,5,12);OLED_ShowString(105,10,"mm");//=============第三行显示编码器1=======================// OLED_ShowString(00,20,"EncoLEFT");if( Encoder_Left<0) OLED_ShowString(80,20,"-"),OLED_ShowNumber(95,20,-Encoder_Left,3,12);else OLED_ShowString(80,20,"+"),OLED_ShowNumber(95,20, Encoder_Left,3,12);//=============第四行显示编码器2=======================// OLED_ShowString(00,30,"EncoRIGHT");if(Encoder_Right<0) OLED_ShowString(80,30,"-"),OLED_ShowNumber(95,30,-Encoder_Right,3,12);else OLED_ShowString(80,30,"+"),OLED_ShowNumber(95,30,Encoder_Right,3,12); //=============第五行显示电压=======================//OLED_ShowString(00,40,"Volta");OLED_ShowString(58,40,".");OLED_ShowString(80,40,"V");OLED_ShowNumber(45,40,Voltage/100,2,12);OLED_ShowNumber(68,40,Voltage%100,2,12);if(Voltage%100<10) OLED_ShowNumber(62,40,0,2,12);//=============第六行显示角度=======================//OLED_ShowString(0,50,"Angle");if(Angle_Balance<0) OLED_ShowNumber(45,50,Angle_Balance+360,3,12);else OLED_ShowNumber(45,50,Angle_Balance,3,12);//=============刷新=======================//OLED_Refresh_Gram(); }
/**************************************************************************
函数功能：向APP发送数据
入口参数：无
返回 值：无
作 者：平衡小车之家
**************************************************************************/
void APP_Show(void)
{ static u8 flag;int app_2,app_3,app_4;app_4=(Voltage-1110)*2/3; if(app_4<0)app_4=0;if(app_4>100)app_4=100; //对电压数据进行处理app_3=Encoder_Right/1.3; if(app_3<0)app_3=-app_3; //对编码器数据就行数据处理便于图形化app_2=Encoder_Left/1.3; if(app_2<0)app_2=-app_2;flag=!flag;if(PID_Send==1)//发送PID参数{printf("{C%d:%d:%d:%d:%d:%d:%d:%d:%d}$",(int)(Balance_Kp*100),(int)(Balance_Kd*100),(int)(Velocity_Kp*100),(int)(Velocity_Ki*100),0,0,0,0,0);//打印到APP上面 PID_Send=0; } else if(flag==0)// printf("{A%d:%d:%d:%d}$",(u8)app_2,(u8)app_3,app_4,(int)Angle_Balance); //打印到APP上面elseprintf("{B%d:%d:%d:%d}$",(int)Angle_Balance,Distance,Encoder_Left,Encoder_Right);//打印到APP上面 显示波形
}
/**************************************************************************
函数功能：虚拟示波器往上位机发送数据 关闭显示屏
入口参数：无
返回 值：无
作 者：平衡小车之家
**************************************************************************/
void DataScope(void)
{ Vol=(float)Voltage/100;DataScope_Get_Channel_Data( Angle_Balance, 1 ); //显示角度 单位：度（°）DataScope_Get_Channel_Data( Distance/10, 2 ); //显示超声波测量的距离 单位：CM DataScope_Get_Channel_Data( Vol, 3 ); //显示电池电压 单位：V
// DataScope_Get_Channel_Data( 0 , 4 );
// DataScope_Get_Channel_Data(0, 5 ); //用您要显示的数据替换0就行了
// DataScope_Get_Channel_Data(0 , 6 );//用您要显示的数据替换0就行了
// DataScope_Get_Channel_Data(0, 7 );
// DataScope_Get_Channel_Data( 0, 8 );
// DataScope_Get_Channel_Data(0, 9 );
// DataScope_Get_Channel_Data( 0 , 10);Send_Count = DataScope_Data_Generate(3);for( i = 0 ; i < Send_Count; i++) {while((USART1->SR&0X40)==0); USART1->DR = DataScope_OutPut_Buffer[i]; }
}
DateScope_DP.c
#include "DataScope_DP.h"/**************************************************************************
作者：平衡小车之家
淘宝店铺：/
**************************************************************************/
unsigned char DataScope_OutPut_Buffer[42] = {0}; //串口发送缓冲区//函数说明：将单精度浮点数据转成4字节数据并存入指定地址
//附加说明：用户无需直接操作此函数
//target:目标单精度数据
//buf:待写入数组
//beg:指定从数组第几个元素开始写入
//函数无返回
void Float2Byte(float *target,unsigned char *buf,unsigned char beg)
{unsigned char *point;point = (unsigned char*)target; //得到float的地址buf[beg] = point[0];buf[beg+1] = point[1];buf[beg+2] = point[2];buf[beg+3] = point[3];
}//函数说明：将待发送通道的单精度浮点数据写入发送缓冲区
//Data：通道数据
//Channel：选择通道（1-10）
//函数无返回
void DataScope_Get_Channel_Data(float Data,unsigned char Channel)
{if ( (Channel > 10) || (Channel == 0) ) return; //通道个数大于10或等于0，直接跳出，不执行函数else{switch (Channel){case 1: Float2Byte(&Data,DataScope_OutPut_Buffer,1); break;case 2: Float2Byte(&Data,DataScope_OutPut_Buffer,5); break;case 3: Float2Byte(&Data,DataScope_OutPut_Buffer,9); break;case 4: Float2Byte(&Data,DataScope_OutPut_Buffer,13); break;case 5: Float2Byte(&Data,DataScope_OutPut_Buffer,17); break;case 6: Float2Byte(&Data,DataScope_OutPut_Buffer,21); break;case 7: Float2Byte(&Data,DataScope_OutPut_Buffer,25); break;case 8: Float2Byte(&Data,DataScope_OutPut_Buffer,29); break;case 9: Float2Byte(&Data,DataScope_OutPut_Buffer,33); break;case 10: Float2Byte(&Data,DataScope_OutPut_Buffer,37); break;}}
}//函数说明：生成 DataScopeV1.0 能正确识别的帧格式
//Channel_Number，需要发送的通道个数
//返回发送缓冲区数据个数
//返回0表示帧格式生成失败
unsigned char DataScope_Data_Generate(unsigned char Channel_Number)
{if ( (Channel_Number > 10) || (Channel_Number == 0) ) { return 0; } //通道个数大于10或等于0，直接跳出，不执行函数else{ DataScope_OutPut_Buffer[0] = '$'; //帧头switch(Channel_Number) { case 1: DataScope_OutPut_Buffer[5] = 5; return 6; case 2: DataScope_OutPut_Buffer[9] = 9; return 10;case 3: DataScope_OutPut_Buffer[13] = 13; return 14; case 4: DataScope_OutPut_Buffer[17] = 17; return 18;case 5: DataScope_OutPut_Buffer[21] = 21; return 22; case 6: DataScope_OutPut_Buffer[25] = 25; return 26;case 7: DataScope_OutPut_Buffer[29] = 29; return 30; case 8: DataScope_OutPut_Buffer[33] = 33; return 34; case 9: DataScope_OutPut_Buffer[37] = 37; return 38;case 10: DataScope_OutPut_Buffer[41] = 41; return 42; } }return 0;
}
更多推荐
平衡车代码阅读,学习mpu6050滤波
发布评论