經過調試可以正確讀取數據并輸出 姿態角
單片機源程序如下:
- /******************** (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, 下載次數: 431)
2017-7-30 01:31 上傳
點擊文件名下載附件
|
[復制鏈接]
