源碼如下

1. #include "sys.h"
   
2. 
   
3. /********************************************************************
   
4. 說明：本程序移植自平衡小車之家的STM32F103C8T6，所用芯片為STM32F4ZGT6
   
5. 該程序僅供學習交流用，請勿用作商業用途
   
6.                                                                                                                                                                                                                                                 2020-1-27
   
7.                                                                                                                                                                                                                                                 ZhOu
   
8. ********************************************************************/
   
9. 
   
10. float Middle=179,Walk_Middle=-0.33;//BALANCE
    
11. float Motor_Balance,Motor_Walk;//MOTOR SPEED
    
12. float        Balance_KP=1800,Balance_KD=8,Velocity_KP=150,Velocity_KI=3,Walk_Balance_KP=450,Walk_Balance_KD=12,Walk_Velocity_KP=175,Walk_Velocity_KI=10; //PID
    
13. float Encoder_Balance,Encoder_Walk;//ENCODER
    
14. float Pitch,Roll,Yaw;   //三軸角度和XYZ軸目標速度
    
15. u8 Flag_Show=0;//OLED SHOW
    
16. u8 delay_50,delay_flag;//DELAY
    
17. char star_flag=0,Flag_Stop=0;
    
18. 
    
19. 
    
20. int main()
    
21. {
    
22.         char key,time=0;
    
23.         NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//設置系統中斷優先級分組2
    
24.         delay_init(168);  //初始化延時函數
    
25.         LED_Init();
    
26.         KEY_Init();
    
27.         uart_init(115200);        //串口初始化波特率為115200
    
28.         TIM4_PWM_Init(10000-1,1-1);
    
29.         TIM9_PWM_Init(10000-1,1-1);
    
30.         Encoder_Init_TIM2();
    
31.         Encoder_Init_TIM3();
    
32.         OLED_Init();
    
33.         OLED_Fill(0x00);
    
34.         IIC_Init();                     //=====IIC初始化
    
35.         MPU6050_initialize();           //=====MPU6050初始化       
    
36.         DMP_Init();                     //=====初始化DMP     
    
37.         MY_EXTI_Init();                    //=====MPU6050 5ms定時中斷初始化
    
38.         TIM_SetTIM4Compare1(0);
    
39.         TIM_SetTIM4Compare2(0);
    
40.         TIM_SetTIM4Compare3(0);
    
41.         TIM_SetTIM4Compare4(0);
    
42.         while(1)
    
43.         {
    
44.                 key=KEY_Scan(0);
    
45.                 KEY_Work(key);
    
46.                 if(time==5)
    
47.                 {
    
48.                         time=0;
    
49.                         OLED_Fill(0x00);
    
50.                         if(star_flag==1) LED1=0;
    
51.                         else if(star_flag==2) LED1=!LED1;
    
52.                 }
    
53.                 if(Flag_Show==0)
    
54.                         oled_show();
    
55.                 else if(Flag_Show==1)
    
56.                         DataScope();
    
57.                 delay_flag=1;       
    
58.                 delay_50=0;
    
59.                 while(delay_flag);             //通過MPU6050的INT中斷實現的50ms精準延時                               
    
60.                 time++;
    
61.         }
    
62. }
    

復制代碼