久久亚洲精品无码av网,欧美大肥婆大肥BBBBB,在线观看欧美精品二区

/******************** (C) COPYRIGHT 2012 WildFire Team **************************
* 文件名：main.c
* 描述：I2C EEPROM(AT24C02)測(cè)試，測(cè)試信息通過USART1打印在電腦的超級(jí)終端。
* 實(shí)驗(yàn)平臺(tái)：野火STM32開發(fā)板
* 庫(kù)版本：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; //平衡環(huán)控制相關(guān)變量
float Encoder_left,Encoder_right; //速度環(huán)控制相關(guān)變量
int Moto1,Moto2;
/*
* 函數(shù)名：main
* 描述：主函數(shù)
* 輸入：無(wú)
* 輸出：無(wú)
* 返回：無(wú)
*/
//中斷分組處理函數(shù)
void NVIC_Configuration(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //設(shè)置NVIC中斷分組2:2位搶占優(yōu)先級(jí)，2位響應(yīng)優(yōu)先級(jí)
}
int Read_Encoder(u8 TIMX);
//位置平衡PID控制
int balance(float Angle,float Gyro)
{
float Bias,kp=500,kd=1;
int balance;
Bias=Angle-ZHONGZHI; //===求出平衡的角度中值和機(jī)械相關(guān)
balance=kp*Bias+Gyro*kd; //===計(jì)算平衡控制的電機(jī)PWM PD控制 kp是P系數(shù) kd是D系數(shù)
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，減緩速度差值，減少對(duì)直立系統(tǒng)的干擾
Encoder+=Encoder_Least*0.3; //一階低通濾波
Encoder_Integral+=Encoder; //積分出位移，積分時(shí)間10ms
Encoder_Integral=Encoder_Integral-Movement; //接受遙控器的數(shù)據(jù)，控制正反轉(zhuǎn)
if(Encoder_Integral>15000){
Encoder_Integral=15000; //積分限幅
}
if(Encoder_Integral<-15000)
{
Encoder_Integral=-15000;
}
Velocity=Encoder*kp+Encoder_Integral*ki; //PI 控制器
return Velocity;
}
//限幅函數(shù)
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;
}
//絕對(duì)值函數(shù)
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時(shí)鐘使能
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端口時(shí)鐘
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); //根據(jù)設(shè)定參數(shù)初始化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(); //初始化電機(jī)控制所需IO
RCC->APB2ENR|=1<<11; //使能TIM1時(shí)鐘
RCC->APB2ENR|=1<<2; //PORTA時(shí)鐘使能
GPIOA->CRH&=0XFFFF0FF0; //PORTA8 11復(fù)用輸出
GPIOA->CRH|=0X0000B00B; //PORTA8 11復(fù)用輸出
TIM1->ARR=arr; //設(shè)定計(jì)數(shù)器自動(dòng)重裝值
TIM1->PSC=psc; //預(yù)分頻器不分頻
TIM1->CCMR2|=6<<12; //CH4 PWM1模式
TIM1->CCMR1|=6<<4; //CH1 PWM1模式
TIM1->CCMR2|=1<<11; //CH4預(yù)裝載使能
TIM1->CCMR1|=1<<3; //CH1預(yù)裝載使能
TIM1->CCER|=1<<12; //CH4輸出使能
TIM1->CCER|=1<<0; //CH1輸出使能
TIM1->BDTR |= 1<<15; //TIM1必須要這句話才能輸出PWM
TIM1->CR1=0x8000; //ARPE使能
TIM1->CR1|=0x01; //使能定時(shí)器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軸加速度計(jì)
int Balance_Pwm,Velocity_Pwm;
NVIC_Configuration(); //設(shè)置NVIC中斷分組2:2位搶占優(yōu)先級(jí)，2位響應(yīng)優(yōu)先級(jí)
/* 串口1初始化 */
USART1_Config();
/*GPIO 口初始化 */
MiniBalance_Motor_Init();
/*定時(shí)器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); //===讀取編碼器的值，因?yàn)閮蓚€(gè)電機(jī)的旋轉(zhuǎn)了180度的，所以對(duì)其中一個(gè)取反，保證輸出極性一致
Encoder_right=Read_Encoder(4);
/*printf("\r\n---------加速度X軸原始數(shù)據(jù)---------%d \r\n",Accel_X);
printf("\r\n---------加速度Y軸原始數(shù)據(jù)---------%d \r\n",Accel_Y);
printf("\r\n---------加速度Z軸原始數(shù)據(jù)---------%d \r\n",Accel_Z);
printf("\r\n---------陀螺儀X軸原始數(shù)據(jù)---------%d \r\n",Gyro_X);
printf("\r\n---------陀螺儀Y軸原始數(shù)據(jù)---------%d \r\n",Gyro_Y);
printf("\r\n---------陀螺儀Z軸原始數(shù)據(jù)---------%d \r\n",Gyro_Z);*/
//delay_ms(500);
if(Gyro_Y>32768) Gyro_Y-=65536; //數(shù)據(jù)類型轉(zhuǎn)換也可通過short強(qiáng)制類型轉(zhuǎn)換
if(Gyro_Z>32768) Gyro_Z-=65536; //數(shù)據(jù)類型轉(zhuǎn)換
if(Accel_X>32768) Accel_X-=65536; //數(shù)據(jù)類型轉(zhuǎn)換
if(Accel_Z>32768) Accel_Z-=65536; //數(shù)據(jù)類型轉(zhuǎn)換
Gyro_Balance=-Gyro_Y; //更新平衡角速度
Accel_Y=atan2(Accel_X,Accel_Z)*180/PI; //計(jì)算傾角
Gyro_Y =Gyro_Y/16.4; //陀螺儀量程轉(zhuǎn)換
Kalman_Filter(Accel_Y,-Gyro_Y);//卡爾曼濾波
//else if(Way_Angle==3) Yijielvbo(Accel_Y,-Gyro_Y); //互補(bǔ)濾波
Angle_Balance=angle; //更新平衡傾角
Gyro_Turn=Gyro_Z; //更新轉(zhuǎn)向角速度
Acceleration_Z=Accel_Z; //===更新Z軸加速度計(jì)
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************/
……………………
…………限于本文篇幅余下代碼請(qǐng)從51黑下載附件…………

復(fù)制代碼