基于STM32的平衡车其程序代码怎样去实现呢

　　#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）;
　　}
　　//
　　}
　　}