平衡之家的程序修改的。
單片機源程序如下:
- #include "sys.h"
- u8 Way_Angle=1; //獲取角度的算法，1：四元數 2：卡爾曼 3：互補濾波 （有的6050使用DMP時，需要開機后不停搖晃小車10S左右，等待數據穩定）
- u8 Flag_Qian,Flag_Hou;
- float Angle_Balance,Gyro_Balance,Acceleration_Z;
- int Moto1,Moto2,Final_Moto1,Final_Moto2; //電機PWM變量
- float Balance_Kp=190,Balance_Kd=0,Velocity_Kp=42,Velocity_Ki=0.21;
- //float Zhongzhi=3.89;
- int Zhongzhi,Flag_Zhongzhi=0;
- int main(void)
- {
- Stm32_Clock_Init(9); //系統時鐘設置
- MY_NVIC_PriorityGroupConfig(2); //=====中斷分組
- delay_init(72); //延時初始化
- uart_init(72,9600); //初始化串口1
- LED_Init();
- IIC_Init(); //模擬IIC初始化
- MPU6050_initialize(); //=====MPU6050初始化
- DMP_Init(); //初始化DMP
- Timer2_Init(50,7200);
- //MiniBalance_EXTI_Init();
- MiniBalance_Motor_Init(); //=====電機方向IO控制初始化
- MiniBalance_PWM_Init(); //=====初始化PWM 輸出
-
- while(1)
- {
- //Read_DMP(); //===讀取傾角
- //printf("Roll:%f ",Roll); //X
- //printf("Pitch:%f ",Pitch); //Y
- //printf("Yaw:%f\r\n",Yaw); //Z
- //delay_ms(10);
-
- }
- }
所有資料51hei提供下載:
步進電機平衡車（DMP）.7z
(219.41 KB, 下載次數: 82)
2019-5-24 18:49 上傳
點擊文件名下載附件
自己修改的代碼
|
