Chen Huan
/
Final_DongGanPingTai
Tsinghua Icenter ChenHuan
Diff: main.cpp
- Revision:
- 0:9b8df4f9b792
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Mar 16 13:07:14 2017 +0000 @@ -0,0 +1,768 @@ +/*************************************************************** +功能 : 基于mbed的动感平台SDK 包含摇杆 位置传感器 舵机 以及液晶屏等外设 +作者 : 陈欢 清华大学电机工程与应用电子技术系 +邮箱 : h-che14@mails.tsinghua.edu.cn OR heroistired@gmail.com +声明 : +本程序仅供学习与交流使用,如需他用,须联系作者 +All rights reserved +2017.2.15 +***************************************************************/ + +#include "mbed.h" +#include "mpu6050_config.h" +#include "Servo.h" +#include "TFT_ILI9340.h" +#include "PinMap.h" +Serial SystemUart(SERIAL_TX, SERIAL_RX); //系统串口初始化 + +////////////////////////////////////////////////////////////////////////////////// +//头文件部分 +////////////////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////// +//系统相关 +/////////////////////////////////////// + +//系统监控 +//接受外部调用的变量 +int TimeNow = 0; //当前的系统时间 +float CpuUsage = 0.0; //当前的CPU使用率 以主中断为基准 +unsigned int MainInterruptCounter = 0; //存储系统主中断的总执行次数 +float MainInterruptFreq = 0.0; //存储系统主中断的实际频率 Hz +//中间变量 不接受外部调用 +int FreqTime = 0; //计算系统中断的实际频率 存储的是测量开始时的系统时间 +int FreqCounter = 0; //计算系统中断的实际频率 存储的是测量开始时的中断执行次数 +bool Flag_MeasureFreq = true; //标志 是否要进行一次频率测量 + + +/////////////////////////////////////// +//mpuiic.h +/////////////////////////////////////// + +//声明IIC需要用到的IO口 +DigitalOut MPU_IIC_SCL(Pin_MPU6050SCL); //SCL +DigitalInOut MPU_IIC_SDA(Pin_MPU6050SDA); //SDA + +//弥补屏幕LED画错的bug +DigitalOut TFT_LED(Pin_TFT_LED); + + +//定义IO操作的宏 +#define MPU_SDA_IN() MPU_IIC_SDA.input() +#define MPU_SDA_OUT() MPU_IIC_SDA.output() +#define MPU_READ_SDA MPU_IIC_SDA.read() //读取SDA + +//兼容性宏定义 +#define delay_us wait_us +#define delay_ms wait_ms +#define u8 unsigned char +#define u16 unsigned short + +//IIC所有操作函数 +void MPU_IIC_Delay(void); //MPU IIC延时函数 +void MPU_IIC_Init(void); //初始化IIC的IO口 +void MPU_IIC_Start(void); //发送IIC开始信号 +void MPU_IIC_Stop(void); //发送IIC停止信号 +void MPU_IIC_Send_Byte(u8 txd); //IIC发送一个字节 +u8 MPU_IIC_Read_Byte(unsigned char ack);//IIC读取一个字节 +u8 MPU_IIC_Wait_Ack(void); //IIC等待ACK信号 +void MPU_IIC_Ack(void); //IIC发送ACK信号 +void MPU_IIC_NAck(void); //IIC不发送ACK信号 + +void IMPU_IC_Write_One_Byte(u8 daddr,u8 addr,u8 data); +u8 MPU_IIC_Read_One_Byte(u8 daddr,u8 addr); + +/////////////////////////////////////// +//mp6050.h +/////////////////////////////////////// +u8 MPU_Init(void); //初始化MPU6050 +u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf);//IIC连续写 +u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf); //IIC连续读 +u8 MPU_Write_Byte(u8 reg,u8 data); //IIC写一个字节 +u8 MPU_Read_Byte(u8 reg); //IIC读一个字节 + +u8 MPU_Set_Gyro_Fsr(u8 fsr); +u8 MPU_Set_Accel_Fsr(u8 fsr); +u8 MPU_Set_LPF(u16 lpf); +u8 MPU_Set_Rate(u16 rate); +u8 MPU_Set_Fifo(u8 sens); + + +short MPU_Get_Temperature(void); +u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz); +u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az); + +/////////////////////////////////////// +//软件方法解算姿态 +/////////////////////////////////////// +Timer SystemTimer; //系统计时器 +Ticker MainInterrupt; //主中断 100Hz +//加速度 角速度 温度(原始值) +short aacx = 0, aacy = 0, aacz = 0, gyrox = 0, gyroy = 0, gyroz = 0, temp = 0; +float Pitch = 0, Yaw = 0, Roll = 0; //绕x,y,z轴的转角 +float ThetaXz = 0, ThetaYz = 0; //重力加速度在XOZ、YOZ面上与z轴夹角 + +//卡尔曼滤波器相关参量 +#define PI 3.1415926 +struct KalmanStruct +{ + float K1; + float angle, angle_dot; + float Q_angle;// 过程噪声的协方差 + float Q_gyro;//0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵 + float R_angle;// 测量噪声的协方差 既测量偏差 + float dt;// + char C_0; + float Q_bias, Angle_err; + float PCt_0, PCt_1, E; + float K_0, K_1, t_0, t_1; + float Pdot[4]; + float PP[2][2]; +}; + +struct KalmanStruct X_Axis = {0}, Y_Axis = {0}; + +void MainInterruptIRQ(void); //主中断服务函数 +void Kalman_Filter(float AccelX,float GyroX, float AccelY,float GyroY); //卡尔曼滤波器 +void Processing(void); //用户进程函数 + +//系统初始化函数 +void System_Init(void); + +////////////////////////////////////////////////////////////////////////////////// +//函数实现部分 +////////////////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////// +//mpuiic.c +/////////////////////////////////////// + +//MPU IIC 延时函数 +void MPU_IIC_Delay(void) +{ + delay_us(2); +} +//初始化IIC +void MPU_IIC_Init(void) +{ + RCC->APB2ENR|=1<<4; //先使能外设IO PORTC时钟 + GPIOC->CRH&=0XFFF00FFF; //PC11/12 推挽输出 + GPIOC->CRH|=0X00033000; + GPIOC->ODR|=3<<11; //PC11,12 输出高 +} +//产生IIC起始信号 +void MPU_IIC_Start(void) +{ + MPU_SDA_OUT(); //sda线输出 + MPU_IIC_SDA=1; + MPU_IIC_SCL=1; + MPU_IIC_Delay(); + MPU_IIC_SDA=0;//START:when CLK is high,DATA change form high to low + MPU_IIC_Delay(); + MPU_IIC_SCL=0;//钳住I2C总线,准备发送或接收数据 +} +//产生IIC停止信号 +void MPU_IIC_Stop(void) +{ + MPU_SDA_OUT();//sda线输出 + MPU_IIC_SCL=0; + MPU_IIC_SDA=0;//STOP:when CLK is high DATA change form low to high + MPU_IIC_Delay(); + MPU_IIC_SCL=1; + MPU_IIC_SDA=1;//发送I2C总线结束信号 + MPU_IIC_Delay(); +} +//等待应答信号到来 +//返回值:1,接收应答失败 +// 0,接收应答成功 +u8 MPU_IIC_Wait_Ack(void) +{ + u8 ucErrTime=0; + MPU_SDA_IN(); //SDA设置为输入 + MPU_IIC_SDA=1;MPU_IIC_Delay(); + MPU_IIC_SCL=1;MPU_IIC_Delay(); + while(MPU_READ_SDA) + { + ucErrTime++; + if(ucErrTime>250) + { + MPU_IIC_Stop(); + return 1; + } + } + MPU_IIC_SCL=0;//时钟输出0 + return 0; +} +//产生ACK应答 +void MPU_IIC_Ack(void) +{ + MPU_IIC_SCL=0; + MPU_SDA_OUT(); + MPU_IIC_SDA=0; + MPU_IIC_Delay(); + MPU_IIC_SCL=1; + MPU_IIC_Delay(); + MPU_IIC_SCL=0; +} +//不产生ACK应答 +void MPU_IIC_NAck(void) +{ + MPU_IIC_SCL=0; + MPU_SDA_OUT(); + MPU_IIC_SDA=1; + MPU_IIC_Delay(); + MPU_IIC_SCL=1; + MPU_IIC_Delay(); + MPU_IIC_SCL=0; +} +//IIC发送一个字节 +//返回从机有无应答 +//1,有应答 +//0,无应答 +void MPU_IIC_Send_Byte(u8 txd) +{ + u8 t; + MPU_SDA_OUT(); + MPU_IIC_SCL=0;//拉低时钟开始数据传输 + for(t=0;t<8;t++) + { + MPU_IIC_SDA=(txd&0x80)>>7; + txd<<=1; + MPU_IIC_SCL=1; + MPU_IIC_Delay(); + MPU_IIC_SCL=0; + MPU_IIC_Delay(); + } +} +//读1个字节,ack=1时,发送ACK,ack=0,发送nACK +u8 MPU_IIC_Read_Byte(unsigned char ack) +{ + unsigned char i,receive=0; + MPU_SDA_IN();//SDA设置为输入 + for(i=0;i<8;i++ ) + { + MPU_IIC_SCL=0; + MPU_IIC_Delay(); + MPU_IIC_SCL=1; + receive<<=1; + if(MPU_READ_SDA)receive++; + MPU_IIC_Delay(); + } + if (!ack) + MPU_IIC_NAck();//发送nACK + else + MPU_IIC_Ack(); //发送ACK + return receive; +} + +/////////////////////////////////////// +//mp6050.c +/////////////////////////////////////// +//初始化MPU6050 +//返回值:0,成功 +// 其他,错误代码 +u8 MPU_Init(void) +{ + u8 res; + MPU_IIC_Init();//初始化IIC总线 + MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050 + delay_ms(100); + MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050 + MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps + MPU_Set_Accel_Fsr(0); //加速度传感器,±2g + MPU_Set_Rate(200); //设置采样率200Hz + MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断 + MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭 + MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO + MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效 + res=MPU_Read_Byte(MPU_DEVICE_ID_REG); + if(res==MPU_ADDR)//器件ID正确 + { + MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考 + MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作 + MPU_Set_Rate(200); //设置采样率为200Hz + }else return 1; + return 0; +} +//设置MPU6050陀螺仪传感器满量程范围 +//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps +//返回值:0,设置成功 +// 其他,设置失败 +u8 MPU_Set_Gyro_Fsr(u8 fsr) +{ + return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围 +} +//设置MPU6050加速度传感器满量程范围 +//fsr:0,±2g;1,±4g;2,±8g;3,±16g +//返回值:0,设置成功 +// 其他,设置失败 +u8 MPU_Set_Accel_Fsr(u8 fsr) +{ + return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围 +} +//设置MPU6050的数字低通滤波器 +//lpf:数字低通滤波频率(Hz) +//返回值:0,设置成功 +// 其他,设置失败 +u8 MPU_Set_LPF(u16 lpf) +{ + u8 data=0; + if(lpf>=188)data=1; + else if(lpf>=98)data=2; + else if(lpf>=42)data=3; + else if(lpf>=20)data=4; + else if(lpf>=10)data=5; + else data=6; + return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器 +} +//设置MPU6050的采样率(假定Fs=1KHz) +//rate:4~1000(Hz) +//返回值:0,设置成功 +// 其他,设置失败 +u8 MPU_Set_Rate(u16 rate) +{ + u8 data; + if(rate>1000)rate=1000; + if(rate<4)rate=4; + data=1000/rate-1; + data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器 + return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半 +} + +//得到温度值 +//返回值:温度值(扩大了100倍) +short MPU_Get_Temperature(void) +{ + u8 buf[2]; + short raw; + float temp; + MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); + raw=((u16)buf[0]<<8)|buf[1]; + temp=36.53+((double)raw)/340; + return temp*100;; +} +//得到陀螺仪值(原始值) +//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号) +//返回值:0,成功 +// 其他,错误代码 +u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz) +{ + u8 buf[6],res; + res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf); + if(res==0) + { + *gx=((u16)buf[0]<<8)|buf[1]; + *gy=((u16)buf[2]<<8)|buf[3]; + *gz=((u16)buf[4]<<8)|buf[5]; + } + return res;; +} +//得到加速度值(原始值) +//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号) +//返回值:0,成功 +// 其他,错误代码 +u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az) +{ + u8 buf[6],res; + res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf); + if(res==0) + { + *ax=((u16)buf[0]<<8)|buf[1]; + *ay=((u16)buf[2]<<8)|buf[3]; + *az=((u16)buf[4]<<8)|buf[5]; + } + return res;; +} +//IIC连续写 +//addr:器件地址 +//reg:寄存器地址 +//len:写入长度 +//buf:数据区 +//返回值:0,正常 +// 其他,错误代码 +u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf) +{ + u8 i; + MPU_IIC_Start(); + MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令 + if(MPU_IIC_Wait_Ack()) //等待应答 + { + MPU_IIC_Stop(); + return 1; + } + MPU_IIC_Send_Byte(reg); //写寄存器地址 + MPU_IIC_Wait_Ack(); //等待应答 + for(i=0;i<len;i++) + { + MPU_IIC_Send_Byte(buf[i]); //发送数据 + if(MPU_IIC_Wait_Ack()) //等待ACK + { + MPU_IIC_Stop(); + return 1; + } + } + MPU_IIC_Stop(); + return 0; +} +//IIC连续读 +//addr:器件地址 +//reg:要读取的寄存器地址 +//len:要读取的长度 +//buf:读取到的数据存储区 +//返回值:0,正常 +// 其他,错误代码 +u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf) +{ + MPU_IIC_Start(); + MPU_IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令 + if(MPU_IIC_Wait_Ack()) //等待应答 + { + MPU_IIC_Stop(); + return 1; + } + MPU_IIC_Send_Byte(reg); //写寄存器地址 + MPU_IIC_Wait_Ack(); //等待应答 + MPU_IIC_Start(); + MPU_IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令 + MPU_IIC_Wait_Ack(); //等待应答 + while(len) + { + if(len==1)*buf=MPU_IIC_Read_Byte(0);//读数据,发送nACK + else *buf=MPU_IIC_Read_Byte(1); //读数据,发送ACK + len--; + buf++; + } + MPU_IIC_Stop(); //产生一个停止条件 + return 0; +} +//IIC写一个字节 +//reg:寄存器地址 +//data:数据 +//返回值:0,正常 +// 其他,错误代码 +u8 MPU_Write_Byte(u8 reg,u8 data) +{ + MPU_IIC_Start(); + MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令 + if(MPU_IIC_Wait_Ack()) //等待应答 + { + MPU_IIC_Stop(); + return 1; + } + MPU_IIC_Send_Byte(reg); //写寄存器地址 + MPU_IIC_Wait_Ack(); //等待应答 + MPU_IIC_Send_Byte(data);//发送数据 + if(MPU_IIC_Wait_Ack()) //等待ACK + { + MPU_IIC_Stop(); + return 1; + } + MPU_IIC_Stop(); + return 0; +} +//IIC读一个字节 +//reg:寄存器地址 +//返回值:读到的数据 +u8 MPU_Read_Byte(u8 reg) +{ + u8 res; + MPU_IIC_Start(); + MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//发送器件地址+写命令 + MPU_IIC_Wait_Ack(); //等待应答 + MPU_IIC_Send_Byte(reg); //写寄存器地址 + MPU_IIC_Wait_Ack(); //等待应答 + MPU_IIC_Start(); + MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);//发送器件地址+读命令 + MPU_IIC_Wait_Ack(); //等待应答 + res=MPU_IIC_Read_Byte(0);//读取数据,发送nACK + MPU_IIC_Stop(); //产生一个停止条件 + return res; +} + +/////////////////////////////////////// +//软件方法解算姿态 +/////////////////////////////////////// + +/************************************************************************** +函数功能:简易卡尔曼滤波 +入口参数:加速度、角速度 +返回 值:无 +**************************************************************************/ + +void Kalman_Filter(float AccelX,float GyroX, float AccelY,float GyroY) +{ + X_Axis.angle+=(GyroX - X_Axis.Q_bias) * X_Axis.dt; //先验估计 + X_Axis.Pdot[0]=X_Axis.Q_angle - X_Axis.PP[0][1] - X_Axis.PP[1][0]; // Pk-先验估计误差协方差的微分 + + X_Axis.Pdot[1]=-X_Axis.PP[1][1]; + X_Axis.Pdot[2]=-X_Axis.PP[1][1]; + X_Axis.Pdot[3]=X_Axis.Q_gyro; + X_Axis.PP[0][0] += X_Axis.Pdot[0] * X_Axis.dt; // Pk-先验估计误差协方差微分的积分 + X_Axis.PP[0][1] += X_Axis.Pdot[1] * X_Axis.dt; // =先验估计误差协方差 + X_Axis.PP[1][0] += X_Axis.Pdot[2] * X_Axis.dt; + X_Axis.PP[1][1] += X_Axis.Pdot[3] * X_Axis.dt; + + X_Axis.Angle_err = AccelX - X_Axis.angle; //zk-先验估计 + + X_Axis.PCt_0 = X_Axis.C_0 * X_Axis.PP[0][0]; + X_Axis.PCt_1 = X_Axis.C_0 * X_Axis.PP[1][0]; + + X_Axis.E = X_Axis.R_angle + X_Axis.C_0 * X_Axis.PCt_0; + + X_Axis.K_0 = X_Axis.PCt_0 / X_Axis.E; + X_Axis.K_1 = X_Axis.PCt_1 / X_Axis.E; + + X_Axis.t_0 = X_Axis.PCt_0; + X_Axis.t_1 = X_Axis.C_0 * X_Axis.PP[0][1]; + + X_Axis.PP[0][0] -= X_Axis.K_0 * X_Axis.t_0; //后验估计误差协方差 + X_Axis.PP[0][1] -= X_Axis.K_0 * X_Axis.t_1; + X_Axis.PP[1][0] -= X_Axis.K_1 * X_Axis.t_0; + X_Axis.PP[1][1] -= X_Axis.K_1 * X_Axis.t_1; + + X_Axis.angle += X_Axis.K_0 * X_Axis.Angle_err; //后验估计 + X_Axis.Q_bias += X_Axis.K_1 * X_Axis.Angle_err; //后验估计 + X_Axis.angle_dot = GyroX - X_Axis.Q_bias; //输出值(后验估计)的微分=角速度 + + Y_Axis.angle+=(GyroY - Y_Axis.Q_bias) * Y_Axis.dt; //先验估计 + Y_Axis.Pdot[0]=Y_Axis.Q_angle - Y_Axis.PP[0][1] - Y_Axis.PP[1][0]; // Pk-先验估计误差协方差的微分 + + Y_Axis.Pdot[1]=-Y_Axis.PP[1][1]; + Y_Axis.Pdot[2]=-Y_Axis.PP[1][1]; + Y_Axis.Pdot[3]=Y_Axis.Q_gyro; + Y_Axis.PP[0][0] += Y_Axis.Pdot[0] * Y_Axis.dt; // Pk-先验估计误差协方差微分的积分 + Y_Axis.PP[0][1] += Y_Axis.Pdot[1] * Y_Axis.dt; // =先验估计误差协方差 + Y_Axis.PP[1][0] += Y_Axis.Pdot[2] * Y_Axis.dt; + Y_Axis.PP[1][1] += Y_Axis.Pdot[3] * Y_Axis.dt; + + Y_Axis.Angle_err = AccelY - Y_Axis.angle; //zk-先验估计 + + Y_Axis.PCt_0 = Y_Axis.C_0 * Y_Axis.PP[0][0]; + Y_Axis.PCt_1 = Y_Axis.C_0 * Y_Axis.PP[1][0]; + + Y_Axis.E = Y_Axis.R_angle + Y_Axis.C_0 * Y_Axis.PCt_0; + + Y_Axis.K_0 = Y_Axis.PCt_0 / Y_Axis.E; + Y_Axis.K_1 = Y_Axis.PCt_1 / Y_Axis.E; + + Y_Axis.t_0 = Y_Axis.PCt_0; + Y_Axis.t_1 = Y_Axis.C_0 * Y_Axis.PP[0][1]; + + Y_Axis.PP[0][0] -= Y_Axis.K_0 * Y_Axis.t_0; //后验估计误差协方差 + Y_Axis.PP[0][1] -= Y_Axis.K_0 * Y_Axis.t_1; + Y_Axis.PP[1][0] -= Y_Axis.K_1 * Y_Axis.t_0; + Y_Axis.PP[1][1] -= Y_Axis.K_1 * Y_Axis.t_1; + + Y_Axis.angle += Y_Axis.K_0 * Y_Axis.Angle_err; //后验估计 + Y_Axis.Q_bias += Y_Axis.K_1 * Y_Axis.Angle_err; //后验估计 + Y_Axis.angle_dot = GyroY - Y_Axis.Q_bias; //输出值(后验估计)的微分=角速度 +} + +////////////////////////////////////////////////////////////////////////////////// +//功能 : 主中断的服务函数 +//参数 : 无 +//返回值 : 无 +////////////////////////////////////////////////////////////////////////////////// + +void MainInterruptIRQ(void) +{ + float cpu_usage = 0; + //系统监控部分代码 + TimeNow = SystemTimer.read_us(); + if(Flag_MeasureFreq) + { + FreqTime = TimeNow; + FreqCounter = MainInterruptCounter; + Flag_MeasureFreq = false; + } + if((MainInterruptCounter - FreqCounter) == 100) + { + MainInterruptFreq = 100.0 * 1000000 / (TimeNow - FreqTime); + Flag_MeasureFreq = true; + } + MainInterruptCounter++; + + + //temp=MPU_Get_Temperature(); //得到温度值 + MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据 + MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据 + if(gyroy>32768) gyroy-=65536; //数据类型转换 也可通过short强制类型转换 + if(gyroz>32768) gyroz-=65536; //数据类型转换 + if(gyrox>32768) gyrox-=65536; //数据类型转换 + if(aacx>32768) aacx-=65536; //数据类型转换 + if(aacz>32768) aacz-=65536; //数据类型转换 + if(aacy>32768) aacy-=65536; //数据类型转换 + + ThetaXz=atan2((double)aacy,(double)aacz)*180/PI; //重力加速度在XOZ、YOZ面上与z轴夹角 + ThetaYz=atan2((double)aacx,(double)aacz)*180/PI; //计算倾角 + gyroy=gyroy/16.4; //陀螺仪量程转换 + gyrox=gyrox/16.4; //陀螺仪量程转换 + Kalman_Filter(ThetaXz,-gyrox, ThetaYz,-gyroy); + + Processing(); //用户进程 + cpu_usage = (float)(SystemTimer.read_us() - TimeNow) / 10000; //计算CPU使用率 + if(cpu_usage > 0) + CpuUsage = cpu_usage; +} + + + +////////////////////////////////////////////////////////////////////////////////// +//功能 : 系统初始化 +//参数 : 无 +//返回值 : 无 +////////////////////////////////////////////////////////////////////////////////// +void System_Init(void) +{ + SystemTimer.start(); + + //TFT.DispInit(); + //TFT.FillScreen(ILI9340_BLACK); + //TFT.SetRotation(3); + //TFT.DrawString("System initing......", 50, 120, 3, ILI9340_WHITE); + + MPU_Init(); + MainInterrupt.attach(&MainInterruptIRQ, 0.01); + + Flag_MeasureFreq = true; //开始一次测量 + + X_Axis.K1 = 0.02; + X_Axis.Q_angle=0.001; + X_Axis.Q_gyro=0.003; + X_Axis.R_angle=0.5; + X_Axis.dt=0.005; + X_Axis.C_0 = 1; + X_Axis.Pdot[0] = 0; + X_Axis.Pdot[1] = 0; + X_Axis.Pdot[2] = 0; + X_Axis.Pdot[3] = 0; + X_Axis.PP[0][0]=1; + X_Axis.PP[0][1]=0; + X_Axis.PP[1][0]=1; + X_Axis.PP[1][1]=0; + + Y_Axis.K1 = 0.02; + Y_Axis.Q_angle=0.001; + Y_Axis.Q_gyro=0.003; + Y_Axis.R_angle=0.5; + Y_Axis.dt=0.005; + Y_Axis.C_0 = 1; + Y_Axis.Pdot[0] = 0; + Y_Axis.Pdot[1] = 0; + Y_Axis.Pdot[2] = 0; + Y_Axis.Pdot[3] = 0; + Y_Axis.PP[0][0]=1; + Y_Axis.PP[0][1]=0; + Y_Axis.PP[1][0]=1; + Y_Axis.PP[1][1]=0; +} + +//此处--推荐--配置外设、定义全局变量 +//-------------------代码块开始 + +//DigitalOut myled(LED1); +AnalogIn LeftJoystickX(Pin_LeftJoystickX); +AnalogIn LeftJoystickY(Pin_LeftJoystickY); +AnalogIn RightJoystickX(Pin_RightJoystickX); +AnalogIn RightJoystickY(Pin_RightJoystickY); + +float LeftJoystickValueX = 0.0, LeftJoystickValueY = 0.0, RightJoystickValueX = 0.0, RightJoystickValueY = 0.0; //摇杆x,y轴的读数 归一化到 0-1 + +Servo Servo1(Pin_Servo1); +Servo Servo2(Pin_Servo2); +Servo Servo3(Pin_Servo3); +Servo Servo4(Pin_Servo4); +Servo Servo5(Pin_Servo5); +Servo Servo6(Pin_Servo6); +Servo Servo7(Pin_Servo7); +Servo Servo8(Pin_Servo8); +Servo Servo9(Pin_Servo9); +Servo Servo10(Pin_Servo10); +Servo Servo11(Pin_Servo11); +Servo Servo12(Pin_Servo12); + +Serial UserUart(PC_10, PC_11); + + +//-------------------代码块结束 + + int i = 0, j; + float degree1 = 45.0, degree2 = 135.0; +////////////////////////////////////////////////////////////////////////////////// +//功能 : 用户进程函数 用于执行用户自定义的后台任务 执行频率是100Hz +//参数 : 无 +//返回值 : 无 +////////////////////////////////////////////////////////////////////////////////// +void Processing(void) +{ + LeftJoystickValueX = LeftJoystickX.read(); + LeftJoystickValueY = LeftJoystickY.read(); + RightJoystickValueX = RightJoystickX.read(); + RightJoystickValueY = RightJoystickY.read(); + if((MainInterruptCounter % 50) == 0) + { + if(i == 0) + { + i = !i; + Servo1.SetDegree(degree1); + Servo2.SetDegree(degree1); + Servo3.SetDegree(degree1); + Servo4.SetDegree(degree1); + Servo5.SetDegree(degree1); + Servo6.SetDegree(degree1); + Servo7.SetDegree(degree1); + Servo8.SetDegree(degree1); + Servo9.SetDegree(degree1); + Servo10.SetDegree(degree1); + Servo11.SetDegree(degree1); + Servo12.SetDegree(degree1); + } + else + { + i = !i; + Servo1.SetDegree(degree2); + Servo2.SetDegree(degree2); + Servo3.SetDegree(degree2); + Servo4.SetDegree(degree2); + Servo5.SetDegree(degree2); + Servo6.SetDegree(degree2); + Servo7.SetDegree(degree2); + Servo8.SetDegree(degree2); + Servo9.SetDegree(degree2); + Servo10.SetDegree(degree2); + Servo11.SetDegree(degree2); + Servo12.SetDegree(degree2); + } + } +} + +int main() +{ + ILI9340_Display TFT = ILI9340_Display(Pin_TFT_MOSI, Pin_TFT_MISO, Pin_TFT_SCLK, Pin_TFT_CS, Pin_TFT_RESET, Pin_TFT_DC); //液晶屏初始化 已知bug 液晶屏类的定义必须在所有DigitalOut类的定义之后否则无法正常工作 + TFT_LED = 1; //点亮TFT背光 + TFT.DispInit(); + TFT.FillScreen(ILI9340_BLACK); + TFT.SetRotation(3); + TFT.DrawString("iCenter", 76, 86, 3, ILI9340_WHITE); + TFT.DrawString("System initing...", 24, 146, 2, ILI9340_WHITE); + System_Init(); + TFT.DrawString("iCenter", 76, 86, 3, ILI9340_BLACK); + TFT.DrawString("System initing...", 24, 146, 2, ILI9340_BLACK); + + while(true) { + printf("CPU Usage : %.2f\r\n", CpuUsage); + printf("Pitch : %.2f\r\n", Y_Axis.angle); + printf("Roll : %.2f\r\n", X_Axis.angle); + printf("LeftJoystickValueX : %.2f\r\n", LeftJoystickValueX); + printf("LeftJoystickValueY : %.2f\r\n", LeftJoystickValueY); + printf("RightJoystickValueX : %.2f\r\n", RightJoystickValueX); + printf("RightJoystickValueY : %.2f\r\n", RightJoystickValueY); + + // Small amount of text to the display. + + + // Do the waiting thang... + wait(0.5); + + } +}