mpu6050.c
#include "MPU6050.h"
#include "IOI2C.h"
#include "usart.h"
/**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
#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;
else
b = 7; // error
return 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); //加速度度最大量程 +-2G
MPU6050_setSleepEnabled(0); //进入工作状态
MPU6050_setI2CMasterModeEnabled(0); //不让MPU6050 控制AUXI2C
MPU6050_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"
/**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
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; //===计算左轮电机最终PWM
Moto2=Balance_Pwm+Velocity_Pwm+Turn_Pwm; //===计算右轮电机最终PWM
Xianfu_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; //===后退标志位置1
else 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; //===积分出位移 积分时间：10ms
Encoder_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 限制在6900
if(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;
}
else
temp=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; //超时不再等待2000ms
if(Acceleration>26000&&(Angle>(-20+ZHONGZHI))&&(Angle<(20+ZHONGZHI))) //条件2，小车是在0度附近被拿起
flag=2;
}
if(flag==2) //第三步
{
if(++count2>100) count2=0,flag=0; //超时不再等待1000ms
if(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"
/**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
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"
/**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
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上面
else
printf("{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"
/**************************************************************************
作者：平衡小车之家
淘宝店铺：http://shop114407458.taobao.com/
**************************************************************************/
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;
}