平衡之家的程序修改的。
单片机源程序如下:
- #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)
|
