#include "stm32f10x.h"
#include "I2C_MPU6050.h"
#include "USART.h"
#include "SysTick.h"
#include "math.h"
#include "app.h"
#include "control.h"
#include "usmart.h"
#include "timer.h"
#include "NRF24L01.h"
#include "spi.h"
#include <stdio.h>
//二階互補濾波
#define pi 3.141592653
float K2 =0.2; // 對加速度計取值的權重
float dt=0.016;//dt為kalman濾波器采樣時間;
float x1,x2,y1;
float m1,m2,n1;
float anglex2,angley2,anglez2;
float Roll=0,Yaw=0,Pitch=0;
float Roll_tar=0,Yaw_tar=0,Pitch_tar=0;
float Roll_o=0,Yaw_o=0,Pitch_o=0;
float Accel_x; //X軸加速度值暫存
float Accel_y; //Y軸加速度值暫存
float Accel_z; //Z軸加速度值暫存
float Gyro_x; //X軸陀螺儀數(shù)據(jù)暫存
float Gyro_y; //Y軸陀螺儀數(shù)據(jù)暫存
float Gyro_z; //Z軸陀螺儀數(shù)據(jù)暫存
float Angle_x_temp; //由加速度計算的x傾斜角度
float Angle_y_temp; //由加速度計算的y傾斜角度
float Angle_z_temp; //由加速度計算的y傾斜角度
float Angle_X_Final; //X最終傾斜角度
float Angle_Y_Final; //Y最終傾斜角度
float Angle_Z_Final; //z最終傾斜角度
long temp;
float Q_jiao;
unsigned char systick;
int16_t gx,gy,gz;//存儲原始數(shù)據(jù)
float ggx,ggy,ggz;//存儲量化后的數(shù)據(jù)
float Ax,Ay,Az;//單位 g(9.8m/s^2)
u16 THROTTLE=1300;
unsigned char tmp_buf[33];
unsigned int motor1,motor2,motor3,motor4;
unsigned char buff[7];
//unsigned int x,y,z;
//double angle;
void Angle_Calcu(void);
void MPU6050(void);
void Erjielvbox(float angle_m,float gyro_m);
void Erjielvboy(float angle_m,float gyro_m);
void Erjielvboz(float angle_m,float gyro_m);
float hmc_measure();
void Init_HMC5883L();
void yaw(void);
void MPU6050()
{
gx=GetData(GYRO_XOUT_H);
gy=GetData(GYRO_YOUT_H);
gz=GetData(GYRO_ZOUT_H);
ggx=gy/65.6; //陀螺儀量程+-500度
ggy=-gx/65.6; //陀螺儀量程處理
ggz=-gz/65.6;
// printf("%f,",ggx);
//// printf("%f,",ggy);
// printf("%f,\r\n",ggz);
}
//NRF24L01接收
void jieshou(void)
{
RX_Mode();
if(NRF24L01_RxPacket(tmp_buf)==0)//一旦接收到信息,則顯示出來.
{
tmp_buf[32]=0;//加入字符串結束符
switch(tmp_buf[0])
{
case '1': if(THROTTLE<1400) {THROTTLE=-500;} break; //停止鍵
case '2': THROTTLE=THROTTLE+5; break; //上升
case '3': THROTTLE=THROTTLE-5; break; //下降
case '4': Q_jiao++;if(Q_jiao>10) {Q_jiao=5;} break; //傾角增大
case '5': Q_jiao--;if(Q_jiao<1) {Q_jiao=0; } break; //傾角減小
case '6': Q_jiao=5; break; //傾角15
case '7': THROTTLE=1350; break;
case '8': Q_jiao=0; break;
case '9': break;
case 'A': Roll_tar=Q_jiao; Pitch_tar=0; break;
case 'B': Roll_tar=0; Pitch_tar=Q_jiao; break;
case 'C': Roll_tar=0; Pitch_tar=-Q_jiao; break;
case 'D': Roll_tar=-Q_jiao; Pitch_tar=0; break;
}
}
}
/*
* 函數(shù)名：main
* 描述 ：主函數(shù)
* 輸入 ：無
* 輸出 ：無
* 返回 ：無
*/
int main(void)
{
SystemInit(); //系統(tǒng)時鐘初始化
Usart_Configuration();//串口初始化配置
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //串口中斷初始化
usmart_dev.init(SystemCoreClock/1000000); //usmart初始化
I2C_MPU6050_Init();
SysTick_Init();
/*時鐘啟動*/
TimeStart();
/*陀螺儀傳感器初始化*/
InitMPU6050();
// Init_HMC5883L();
Pid_init(60,0,34,340,7,0); //24
All_GPIO_Config();//所有GPIO配置
TIM3_Config();//TIM2初始化配置
NRF24L01_Init(); //初始化NRF24L01
RX_Mode();
while(NRF24L01_Check())//檢測不到24L01
{
printf("nRF24L01檢測出錯!請確認nRF24L01的連接! \n\r");
delay_ms(1000);
}
printf("\n\r--------------------------------------");
printf("\n\r 神舟III號2.4G無線模塊實驗程序");
printf("\n\r\n\r 請設置NRF24L01無線模塊的工作模式");
printf("\n\r --USER1 按鍵:設置NRF24L01為接收模式");
printf("\n\r --USER2 按鍵:設置NRF24L01為發(fā)送模式");
printf("\n\r --TAMEPR按鍵:退出NRF24L01發(fā)送接收");
printf("\n\r--------------------------------------");
//motor需從1000開始向上加，否則無法啟動
motor1=motor2=motor3=motor4=1000; time(); delay_ms(1000);
motor1=motor2=motor3=motor4=1100; time(); delay_ms(1000);
motor1=motor2=motor3=motor4=1200; time(); delay_ms(1000);
motor1=motor2=motor3=motor4=1300; time(); delay_ms(1000);
while(1)
{
// 關閉滴答定時器
SysTick->CTRL &= ~ SysTick_CTRL_ENABLE_Msk;
printf("%d,",systick);
systick=0;
//使能
SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk;
// Angle_z_temp=hmc_measure();
// MPU6050();
// Angle_Calcu();
// jieshou();
// Control(Roll,Pitch,Yaw,Roll_tar,Pitch_tar,Yaw_tar);//4通道 油門+方向
// time();
}
}
//角度計算
void Angle_Calcu(void)
{
float x,y,z;
Accel_x = GetData(ACCEL_XOUT_H); //x軸加速度值暫存
Accel_y = GetData(ACCEL_YOUT_H); //y軸加速度值暫存
Accel_z = GetData(ACCEL_ZOUT_H); //z軸加速度值暫存
Gyro_x = GetData(GYRO_XOUT_H); //x軸陀螺儀值暫存
Gyro_y = GetData(GYRO_YOUT_H); //y軸陀螺儀值暫存
Gyro_z = GetData(GYRO_ZOUT_H); //z軸陀螺儀值暫存
//處理x軸加速度
if(Accel_x<32764) x=Accel_x/16384;
else x=1-(Accel_x-49152)/16384;
//處理y軸加速度
if(Accel_y<32764) y=Accel_y/16384;
else y=1-(Accel_y-49152)/16384;
//處理z軸加速度
if(Accel_z<32764) z=Accel_z/16384;
else z=(Accel_z-49152)/16384;
//用加速度計算三個軸和水平面坐標系之間的夾角
Angle_x_temp=(atan(y/z))*180/pi;
Angle_y_temp=(atan(x/z))*180/pi;
//角度的正負號
if(Accel_x<32764) Angle_y_temp = +Angle_y_temp;
if(Accel_x>32764) Angle_y_temp = -Angle_y_temp;
if(Accel_y<32764) Angle_x_temp = +Angle_x_temp;
if(Accel_y>32764) Angle_x_temp = -Angle_x_temp;
//向前運動
if(Gyro_x<32768) Gyro_x=+(Gyro_x/16.4);//范圍為1000deg/s時，換算關系：16.4 LSB/(deg/s)
//向后運動
if(Gyro_x>32768) Gyro_x=-(65535-Gyro_x)/16.4;
//向前運動
if(Gyro_y<32768) Gyro_y=+(Gyro_y/16.4);//范圍為1000deg/s時，換算關系：16.4 LSB/(deg/s)
//向后運動
if(Gyro_y>32768) Gyro_y=-(65535-Gyro_y)/16.4;
Erjielvbox(Angle_x_temp,Gyro_x);
Erjielvboy(Angle_y_temp,-Gyro_y);
Erjielvboz(Angle_z_temp,ggz);
Pitch=-anglex2-1;
Roll=-angley2+2.2;
Yaw= anglez2-180;
// printf("%f,",Yaw);
// printf("%f,",Roll);
// printf("%f,\r\n",Pitch);
//// printf("%f,",Yaw);
}
void Erjielvbox(float angle_m,float gyro_m)
{
x1=(angle_m-anglex2)*(1-K2)*(1-K2);
y1=y1+x1*dt;
x2=y1+2*(1-K2)*(angle_m-anglex2)+gyro_m;
anglex2=anglex2+ x2*dt;
}
void Erjielvboy(float angle_m,float gyro_m)
{
m1=(angle_m-angley2)*(1-K2)*(1-K2);
n1=n1+m1*dt;
m2=n1+2*(1-K2)*(angle_m-angley2)+gyro_m;
angley2=angley2+ m2*dt;
}
void Erjielvboz(float angle_m,float gyro_m)
{
m1=(angle_m-anglez2)*(1-K2)*(1-K2);
n1=n1+m1*dt;
m2=n1+2*(1-K2)*(angle_m-anglez2)+gyro_m;
anglez2=anglez2+ m2*dt;
}
|
