经过调试可以正确读取数据并输出 姿态角
单片机源程序如下:
- /******************** (C) COPYRIGHT 2012 WildFire Team **************************
- * 文件名 ：main.c
- * 描述 ：I2C EEPROM(AT24C02)测试，测试信息通过USART1打印在电脑的超级终端。
- * 实验平台：野火STM32开发板
- * 库版本 ：ST3.0.0
- * 作者 ：wildfire team
- **********************************************************************************/
- #include "stm32f10x.h"
- #include "I2C_MPU6050.h"
- #include "usart1.h"
- #include "delay.h"
- #include "filter.h"
- #include "math.h"
- #include "misc.h"
- #include "TIMX.h"
- #define AIN2 PBout(15)
- #define AIN1 PBout(14)
- #define BIN1 PBout(13)
- #define BIN2 PBout(12)
- #define BITBAND(addr, bitnum) ((addr & 0xF0000000)+0x2000000+((addr &0xFFFFF)<<5)+(bitnum<<2))
- #define MEM_ADDR(addr) *((volatile unsigned long *)(addr))
- #define BIT_ADDR(addr, bitnum) MEM_ADDR(BITBAND(addr, bitnum))
- #define GPIOA_ODR_Addr (GPIOA_BASE+12) //0x4001080C
- #define GPIOB_ODR_Addr (GPIOB_BASE+12) //0x40010C0C
- #define GPIOC_ODR_Addr (GPIOC_BASE+12) //0x4001100C
- #define GPIOD_ODR_Addr (GPIOD_BASE+12) //0x4001140C
- #define GPIOE_ODR_Addr (GPIOE_BASE+12) //0x4001180C
- #define GPIOF_ODR_Addr (GPIOF_BASE+12) //0x40011A0C
- #define GPIOG_ODR_Addr (GPIOG_BASE+12) //0x40011E0C
- //#define PAout(n) BIT_ADDR(GPIOA_ODR_Addr,n) //输出
- //#define PAin(n) BIT_ADDR(GPIOA_IDR_Addr,n) //输入
- #define PBout(n) BIT_ADDR(GPIOB_ODR_Addr,n) //输出
- //#define PBin(n) BIT_ADDR(GPIOB_IDR_Addr,n) //输入
- //#define PCout(n) BIT_ADDR(GPIOC_ODR_Addr,n) //输出
- //#define PCin(n) BIT_ADDR(GPIOC_IDR_Addr,n) //输入
- //#define PDout(n) BIT_ADDR(GPIOD_ODR_Addr,n) //输出
- //#define PDin(n) BIT_ADDR(GPIOD_IDR_Addr,n) //输入
- //#define PEout(n) BIT_ADDR(GPIOE_ODR_Addr,n) //输出
- //#define PEin(n) BIT_ADDR(GPIOE_IDR_Addr,n) //输入
- //#define PFout(n) BIT_ADDR(GPIOF_ODR_Addr,n) //输出
- //#define PFin(n) BIT_ADDR(GPIOF_IDR_Addr,n) //输入
- //#define PGout(n) BIT_ADDR(GPIOG_ODR_Addr,n) //输出
- //#define PGin(n) BIT_ADDR(GPIOG_IDR_Addr,n) //输入
- #define PI 3.14159265
- #define ZHONGZHI -6
- #define PWMA TIM1->CCR1 //PA8
- #define PWMB TIM1->CCR4 //PA11
- float Angle_Balance,Gyro_Balance,Gyro_Turn; //平衡环控制相关变量
- float Encoder_left,Encoder_right; //速度环控制相关变量
- int Moto1,Moto2;
- /*
- * 函数名：main
- * 描述 ：主函数
- * 输入 ：无
- * 输出 ：无
- * 返回 ：无
- */
-
- //中断分组处理函数
- void NVIC_Configuration(void)
- {
- NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //设置NVIC中断分组2:2位抢占优先级，2位响应优先级
- }
- int Read_Encoder(u8 TIMX);
- //位置平衡PID控制
- int balance(float Angle,float Gyro)
- {
- float Bias,kp=500,kd=1;
- int balance;
- Bias=Angle-ZHONGZHI; //===求出平衡的角度中值 和机械相关
- balance=kp*Bias+Gyro*kd; //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数
- return balance;
- }
- //速度PI控制
- int velocity(int encoder_left,int encoder_right)
- {
- static float Velocity,Encoder_Least,Encoder,Movement;
- static float Encoder_Integral,Target_Velocity;
- float kp=50,ki=kp/200;
- Encoder_Least=(Encoder_left+Encoder_right)-0;
- Encoder*=0.7; //一阶低通滤波，上次速度差占70，本次速度差占30，减缓速度差值，减少对直立系统的干扰
- Encoder+=Encoder_Least*0.3; //一阶低通滤波
- Encoder_Integral+=Encoder; //积分出位移，积分时间10ms
- Encoder_Integral=Encoder_Integral-Movement; //接受遥控器的数据，控制正反转
- if(Encoder_Integral>15000){
- Encoder_Integral=15000; //积分限幅
- }
- if(Encoder_Integral<-15000)
- {
- Encoder_Integral=-15000;
- }
- Velocity=Encoder*kp+Encoder_Integral*ki; //PI 控制器
-
- return Velocity;
-
- }
- //限幅函数
- void Xianfu_Pwm(void)
- {
- int Amplitude=6900; //===PWM满幅是7200 限制在6900
- if(Moto1<-Amplitude) Moto1=-Amplitude;
- if(Moto1>Amplitude) Moto1=Amplitude;
- if(Moto2<-Amplitude) Moto2=-Amplitude;
- if(Moto2>Amplitude) Moto2=Amplitude;
- }
- //绝对值函数
- int myabs(int a)
- {
- int temp;
- if(a<0) temp=-a;
- else temp=a;
- return temp;
- }
- /*void MiniBalance_Motor_Init(void)
- {
- RCC->APB2ENR|=1<<3; //PORTB时钟使能
- GPIOB->CRH&=0X0000FFFF; //PORTB12 13 14 15推挽输出
- GPIOB->CRH|=0X22220000; //PORTB12 13 14 15推挽输出
- }*/
- void MiniBalance_Motor_Init(void)
- {
-
- GPIO_InitTypeDef GPIO_InitStructure;
-
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); //使能PB,PE端口时钟
-
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15; //LED0-->PB.5 端口配置
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
- GPIO_Init(GPIOB, &GPIO_InitStructure); //根据设定参数初始化GPIOB.5
- //GPIO_SetBits(GPIOB,GPIO_Pin_5);
- //GPIO_ResetBits(GPIOB,GPIO_Pin_6); //PB.5 输出高
- //GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5; //LED1-->PE.5 端口配置, 推挽输出
- //GPIO_Init(GPIOE, &GPIO_InitStructure); //推挽输出 ，IO口速度为50MHz
- //GPIO_SetBits(GPIOE,GPIO_Pin_5); //PE.5 输出高
- }
- void MiniBalance_PWM_Init(u16 arr,u16 psc)
- {
- MiniBalance_Motor_Init(); //初始化电机控制所需IO
- RCC->APB2ENR|=1<<11; //使能TIM1时钟
- RCC->APB2ENR|=1<<2; //PORTA时钟使能
- GPIOA->CRH&=0XFFFF0FF0; //PORTA8 11复用输出
- GPIOA->CRH|=0X0000B00B; //PORTA8 11复用输出
- TIM1->ARR=arr; //设定计数器自动重装值
- TIM1->PSC=psc; //预分频器不分频
- TIM1->CCMR2|=6<<12; //CH4 PWM1模式
- TIM1->CCMR1|=6<<4; //CH1 PWM1模式
- TIM1->CCMR2|=1<<11; //CH4预装载使能
- TIM1->CCMR1|=1<<3; //CH1预装载使能
- TIM1->CCER|=1<<12; //CH4输出使能
- TIM1->CCER|=1<<0; //CH1输出使能
- TIM1->BDTR |= 1<<15; //TIM1必须要这句话才能输出PWM
- TIM1->CR1=0x8000; //ARPE使能
- TIM1->CR1|=0x01; //使能定时器1
- }
- //PWM shewzhi
- void Set_Pwm(int moto1,int moto2)
- {
-
- int siqu=400;
- if(moto1<0)
- {
- printf("AIN反向");
- AIN1=0;
- AIN2=1;
- }
- else
- {
- printf("AINfanxaing");
- AIN2=0;
- AIN1=1;
- }
- PWMA=myabs(moto1)+siqu;
- if(moto2<0)
- {
- BIN1=0;
- BIN2=1;
- }
- else
- {
- BIN1=1;
- BIN2=0;
- }
- PWMB=myabs(moto2)+siqu;
- }
- int main(void)
- {
- int Accel_Y,Accel_X,Accel_Z,Gyro_X,Gyro_Y,Gyro_Z;
- float Acceleration_Z; //Z轴加速度计
- int Balance_Pwm,Velocity_Pwm;
-
- NVIC_Configuration(); //设置NVIC中断分组2:2位抢占优先级，2位响应优先级
-
- /* 串口1初始化 */
- USART1_Config();
- /*GPIO 口初始化 */
- MiniBalance_Motor_Init();
- /*定时器1初始化*/
- MiniBalance_PWM_Init(7199,0);
- Encoder_Init_TIM2(); //=====编码器接口
- Encoder_Init_TIM4(); //=====初始化编码器2
- /*IIC接口初始化*/
- I2C_MPU6050_Init();
- /*陀螺仪传感器初始化*/
- InitMPU6050();
-
- /***********************************************************************/
-
- while(1)
- {
- Accel_X=GetData(ACCEL_XOUT_H);
- Accel_Y=GetData(ACCEL_YOUT_H);
- Accel_Z=GetData(ACCEL_ZOUT_H);
- Gyro_X=GetData(GYRO_XOUT_H);
- Gyro_Y=GetData(GYRO_YOUT_H);
- Gyro_Z=GetData(GYRO_ZOUT_H);
- Encoder_left=-Read_Encoder(2); //===读取编码器的值，因为两个电机的旋转了180度的，所以对其中一个取反，保证输出极性一致
- Encoder_right=Read_Encoder(4);
-
- /*printf("\r\n---------加速度X轴原始数据---------%d \r\n",Accel_X);
- printf("\r\n---------加速度Y轴原始数据---------%d \r\n",Accel_Y);
- printf("\r\n---------加速度Z轴原始数据---------%d \r\n",Accel_Z);
- printf("\r\n---------陀螺仪X轴原始数据---------%d \r\n",Gyro_X);
- printf("\r\n---------陀螺仪Y轴原始数据---------%d \r\n",Gyro_Y);
- printf("\r\n---------陀螺仪Z轴原始数据---------%d \r\n",Gyro_Z);*/
- //delay_ms(500);
-
- if(Gyro_Y>32768) Gyro_Y-=65536; //数据类型转换 也可通过short强制类型转换
- if(Gyro_Z>32768) Gyro_Z-=65536; //数据类型转换
- if(Accel_X>32768) Accel_X-=65536; //数据类型转换
- if(Accel_Z>32768) Accel_Z-=65536; //数据类型转换
- Gyro_Balance=-Gyro_Y; //更新平衡角速度
- Accel_Y=atan2(Accel_X,Accel_Z)*180/PI; //计算倾角
- Gyro_Y =Gyro_Y/16.4; //陀螺仪量程转换
- Kalman_Filter(Accel_Y,-Gyro_Y);//卡尔曼滤波
- //else if(Way_Angle==3) Yijielvbo(Accel_Y,-Gyro_Y); //互补滤波
- Angle_Balance=angle; //更新平衡倾角
- Gyro_Turn=Gyro_Z; //更新转向角速度
- Acceleration_Z=Accel_Z; //===更新Z轴加速度计
- Gyro_Balance=-Gyro_Y;
- printf("卡尔曼滤波值%f,%f\n",Angle_Balance,Gyro_Turn);
- //Balance_Pwm =balance(Angle_Balance,Gyro_Balance);
- Velocity_Pwm=velocity(Encoder_left,Encoder_right);
- Moto1=Velocity_Pwm;
- Moto2=Velocity_Pwm;
- printf("%d,%d\n",Moto1,Moto2);
- Xianfu_Pwm();
- Set_Pwm(Moto1,Moto2);
- delay_ms(5);
- }
-
- }
- /******************* (C) COPYRIGHT 2012 WildFire Team *****END OF FILE************/
- ……………………
- …………限于本文篇幅 余下代码请从51黑下载附件…………
所有资料51hei提供下载:
卡尔曼滤波输出姿态角.zip
(2.14 MB, 下载次数: 429)
|
[复制链接]
