#include "stm32f10x.h" // Device header
#include "Delay.h"
#include "OLED.h"
#include "Serial.h"
#include <stdio.h>
#include <stdlib.h>
#include "Key.h"
#include "sys.h"
#include "LED.h"
#include "Servo.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
unsigned int KeyNum,Mode;
float Ang1,Ang2,AngFlag;
float Angle1=160,Angle2=70;
float Pitch,Roll,Yaw,Pitch_read,Roll_read,Yaw_read;
int16_t Gx,Gy,Gz;
float low_rc(float x,float alpha);
int main(void)
{
OLED_Init();
Key_Init();
Serial_Init();
Servo_Init();
OLED_ShowString(1,1,"Roll:");
OLED_ShowString(2,1,"Yaw:");
OLED_ShowString(1,12,".");
OLED_ShowString(2,12,".");
OLED_ShowString(3,1,"1:");
OLED_ShowString(4,1,"2:");
Servo_SetAngle(160,70);
MPU6050_DMP_Init();
Delay_ms(2000);
while (1)
{
KeyNum=Key_GetNum();
if(KeyNum==1)Mode=0;
if(KeyNum==2)Mode=1;
OLED_ShowSignedNum(1,6,Roll,5);
OLED_ShowSignedNum(2,6,Yaw,5);
OLED_ShowNum(1,13,(int)(Roll*10)%10,1);
OLED_ShowNum(2,13,(int)(Yaw*10)%10,1);
OLED_ShowNum(3,3,Angle1,3);
OLED_ShowNum(4,3,Angle2,3);
if(MPU6050_DMP_Get_Data(&Pitch_read,&Roll_read,&Yaw_read)==0)
{
Roll=Roll_read;if(Roll<-50)Roll=-50;if(Roll>50)Roll=50;
Yaw=Yaw_read;if(Yaw<-160)Yaw=-160;if(Yaw>20)Yaw=20;
}
if(Mode==1)
{
Angle1=160+Yaw;
Angle2=70+Roll;
if(Angle1<0){Angle1=0;}
if(Angle1>180){Angle1=180;}
if(Angle2<0){Angle2=0;}
if(Angle2>180){Angle2=180;}
}
Servo_SetAngle(Angle1,Angle2);
}
}
float low_rc(float x,float alpha)
{
static float y,last_y;
y=alpha*x+(1-alpha)*last_y;
last_y=y;
return y;
}
|
