#include "stm32f10x.h"

#include "sys.h"

#include "PWM.h"

#include "delay.h"

#include "usart.h"

#include "mpu6050.h"

#include "inv_mpu.h"

#include "inv_mpu_dmp_motion_driver.h"

#include "LED.h"

#include "encode.h"

#include "oled.h"

#include "pid.h"

#include "timer.h"

#include "key.h"

extern float Count;  ///编码器故障，展示只有编码器2有用

extern float Count2;

extern float countD_value;

extern float Old_Count;

extern float Old_Count2;

extern float countD_value2;

extern float count_AND;

u16        Parameter = 1881;                                                                                                                                        //输入编码器线数

//u8 t=0,report=1;                        //默认开启上报

u8 key;

float pitch,roll,yaw;                 //欧拉角

short aacx,aacy,aacz;                //加速度传感器原始数据

short gyrox,gyroy,gyroz;        //陀螺仪原始数据

short temp;                                        //温度

float PIDoutALL;

void int_all(void);

void Get_Data(void);

int main(void)

{

int_all();

while(1)

Get_Data();

}

void int_all(void)

{

uart_init(115200);

NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);         //设置NVIC中断分组2:2位抢占优先级，2位响应优先级

SystemInit();

delay_init();

NVIC_Cfg();

GPIO_Cfg();

TIM1_Init();

TIME2_INT(10,7199);//1ms

TIM_Cfg();

TIM_Cmd(TIM3,ENABLE);

TIM4_Init();

OLED_Init();

MPU_Init();                                        //初始化MPU6050

LED1_INT();

Angle_PID_init();

Speed_PID_init();

KEY_Init();

}

void Get_Data(void)

{

//        u8 t=0,report=1;                        //默认开启上报

//        u8 key;

//        float pitch,roll,yaw;                 //欧拉角

//        short aacx,aacy,aacz;                //加速度传感器原始数据

//        short gyrox,gyroy,gyroz;        //陀螺仪原始数据

//        short temp;                                        //温度

//        NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);         //设置NVIC中断分组2:2位抢占优先级，2位响应优先级

//        delay_init();        //延时初始化

//        usmart_dev.init(72);                //初始化USMART

//        LED_Init();                                          //初始化与LED连接的硬件接口

//        KEY_Init();                                        //初始化按键

//        LCD_Init();                                           //初始化LCD

//        MPU_Init();                                        //初始化MPU6050

LED1=1;

LED2=1;

OLED_ShowStr(16, 0,"Balance Car",2);

//  OLED_ON();

//        while(mpu_dmp_init())

//         {

LED2=0;

OLED_ShowStr(0, 2,"Int MPU6050 . ",2);

delay_ms(300);

OLED_ShowStr(0, 2,"Int MPU6050 ..",2);

delay_ms(300);

OLED_ShowStr(0, 2,"Int MPU6050 ...",2);

delay_ms(300);

OLED_ShowStr(0, 2,"               ",2);

//        }

OLED_ShowStr(0, 2,"MPU6050 Check Ok",2);

//        LCD_ShowString(30,130,200,16,16,"MPU6050 OK");

//        LCD_ShowString(30,150,200,16,16,"KEY0:UPLOAD ON/OFF");

//        POINT_COLOR=BLUE;//设置字体为蓝色

//         LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");

//         LCD_ShowString(30,200,200,16,16," Temp:    . C");

//         LCD_ShowString(30,220,200,16,16,"Pitch:    . C");

//         LCD_ShowString(30,240,200,16,16," Roll:    . C");

//         LCD_ShowString(30,260,200,16,16," Yaw :    . C");

while(1)

{

key=KEY_Scan(0);

//                if(key==KEY0_PRES)

//                {

//                        report=!report;

//                        if(report)LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");

//                        else LCD_ShowString(30,170,200,16,16,"UPLOAD OFF");

//                }

LED1=1;

if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)

{

temp=MPU_Get_Temperature();        //得到温度值

MPU_Get_Accelerometer(&aacx,&aacy,&aacz);        //得到加速度传感器数据

MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);        //得到陀螺仪数据

//                        if(report)mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);//用自定义帧发送加速度和陀螺仪原始数据

//                        if(report)usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));

//            if((t%10)==0)

//                   {

pid.Pv=pitch;         //角度PID输入

pid2.Pv2=count_AND;   //速度PID输入

Angle_PID_Calc();    //角度PID计算

Speed_PID_Calc();    //速度PID计算

PIDoutALL=pid2.OUT2+pid.OUT; //双环PID合成

if(PIDoutALL<0)

ALL_right(-PIDoutALL+13);

if(PIDoutALL>0)

ALL_left(PIDoutALL+13);

//                                if(temp<0)

//                                {

////                                        LCD_ShowChar(30+48,200,'-',16,0);                //显示负号

//                                        temp=-temp;                //转为正数

////                                }else LCD_ShowChar(30+48,200,' ',16,0);                //去掉负号

////                                LCD_ShowNum(30+48+8,200,temp/100,3,16);                //显示整数部分

////                                LCD_ShowNum(30+48+40,200,temp%10,1,16);                //显示小数部分

//                                temp=pitch*10;

//

//

//                                if(temp<0)

//                                {

////                                        LCD_ShowChar(30+48,220,'-',16,0);                //显示负号

//                                        temp=-temp;                //转为正数

//                                }else LCD_ShowChar(30+48,220,' ',16,0);                //去掉负号

//                                LCD_ShowNum(30+48+8,220,temp/10,3,16);                //显示整数部分

//                                LCD_ShowNum(30+48+40,220,temp%10,1,16);                //显示小数部分

//                                temp=roll*10;

//

//

//                                if(temp<0)

//                                {

//                                        LCD_ShowChar(30+48,240,'-',16,0);                //显示负号

//                                        temp=-temp;                //转为正数

//                                }else LCD_ShowChar(30+48,240,' ',16,0);                //去掉负号

//                                LCD_ShowNum(30+48+8,240,temp/10,3,16);                //显示整数部分

//                                LCD_ShowNum(30+48+40,240,temp%10,1,16);                //显示小数部分

//                                temp=yaw*10;

//

//

//

//                                if(temp<0)

//                                {

//                                        LCD_ShowChar(30+48,260,'-',16,0);                //显示负号

//                                        temp=-temp;                //转为正数

//                                }else LCD_ShowChar(30+48,260,' ',16,0);                //去掉负号

//                                LCD_ShowNum(30+48+8,260,temp/10,3,16);                //显示整数部分

//                                LCD_ShowNum(30+48+40,260,temp%10,1,16);                //显示小数部分

//                                t=0;

////                                LED0=!LED0;//LED闪烁

//                  }

}

//                t++;

if(Old_Count != Count)                                                                                                                //如果先前的计数值与当前计数值不相等，说明编码器已转动打印当前数值

{

LED1=0;

OLED_ShowStr(0, 0,"AUTO Parking Station",1);

}

//

}

}