亚洲日韩精品欧美中文字幕,成人好吊妞在线播放,无码成a∧人片在线播放

#include "stm32f10x.h"
#include "MPU6050.h"
#include "motor.h"
#include "control.h"
#include <math.h>
//******角度參數(shù)************
float Gyro_y; //Y軸陀螺儀數(shù)據(jù)暫存
float Angle_gy; //由角速度計(jì)算的傾斜角度
float Accel_x; //X軸加速度值暫存
float Angle_ax; //由加速度計(jì)算的傾斜角度
float Angle; //小車最終傾斜角度
//******PWM參數(shù)*************
int speed_mr; //右電機(jī)轉(zhuǎn)速
int speed_ml; //左電機(jī)轉(zhuǎn)速
float PWM; //綜合PWM計(jì)算
//******電機(jī)參數(shù)*************
float speed_r_l; //電機(jī)轉(zhuǎn)速
float speed; //電機(jī)轉(zhuǎn)速濾波
//******卡爾曼參數(shù)************
const float Q_angle=0.001;
const float Q_gyro=0.003;
const float R_angle=0.5;
const float dt=0.01; //dt為kalman濾波器采樣時(shí)間;
const char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
static const float Kp = 35.0; //PID參數(shù)
static const float Kd = 2.5; //PID參數(shù)
static const float Ksp = 10.0; //PID參數(shù)
void TIM4_Configuration(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;//定義一個(gè)定時(shí)器結(jié)構(gòu)體變量
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);//使能定時(shí)器4
TIM_TimeBaseStructure.TIM_Period = (10000 - 1); //計(jì)數(shù)10000次，因?yàn)閺?開(kāi)始，所以減1
TIM_TimeBaseStructure.TIM_Prescaler = (72 - 1); //時(shí)鐘72分頻，因?yàn)?不分頻，所以減1
TIM_TimeBaseStructure.TIM_ClockDivision = 0; // 使用的采樣頻率之間的分頻比例
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //向上計(jì)數(shù)
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); //初始化定時(shí)器4
TIM_ClearITPendingBit(TIM4, TIM_IT_Update); //清除定時(shí)器4中斷標(biāo)志位
TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE); //打開(kāi)定時(shí)器4中斷
TIM_Cmd(TIM4, ENABLE); //計(jì)數(shù)器使能，開(kāi)始計(jì)數(shù)
}
void NVIC_Configuration(void)
{
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0000);
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
/********************注冊(cè)定時(shí)器4中斷處理函數(shù)***********************/
NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
/******************注冊(cè)串口1中斷服務(wù)函數(shù)************************/
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//配置串口中斷
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;//占先式優(yōu)先級(jí)設(shè)置為0
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; //副優(yōu)先級(jí)設(shè)置為0
NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//中斷禁止
NVIC_Init(&NVIC_InitStructure);//中斷初始化
}
void Control_Init(void)
{
TIM4_Configuration();
NVIC_Configuration();
}
/****************定時(shí)器4中斷服務(wù)函數(shù)***************************************/
void TIM4_IRQHandler(void)
{
if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
Angle_Calculate();// 車體傾角計(jì)算
Speed_Calculate();//車速計(jì)算
PWM_Calculate();//電機(jī)PWM值計(jì)算
Motor_Control(PWM);//調(diào)節(jié)電機(jī)參數(shù)
}
}
/*****************卡爾曼濾波**************************************************/
void Kalman_Filter(float Accel,float Gyro)
{
Angle+=(Gyro - Q_bias) * dt; //先驗(yàn)估計(jì)
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先驗(yàn)估計(jì)誤差協(xié)方差的微分
Pdot[1]= -PP[1][1];
Pdot[2]= -PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先驗(yàn)估計(jì)誤差協(xié)方差微分的積分
PP[0][1] += Pdot[1] * dt; // =先驗(yàn)估計(jì)誤差協(xié)方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle; //zk-先驗(yàn)估計(jì)
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后驗(yàn)估計(jì)誤差協(xié)方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle += K_0 * Angle_err; //后驗(yàn)估計(jì)
Q_bias += K_1 * Angle_err; //后驗(yàn)估計(jì)
Gyro_y = Gyro - Q_bias; //輸出值(后驗(yàn)估計(jì))的微分=角速度
}
void Angle_Calculate(void)
{
static uint8_t DataBuffer[2]; //數(shù)據(jù)緩存
/****************************加速度****************************************/
I2C_ReadBuffer(DataBuffer, ACCEL_XOUT_H, 2);
Accel_x = (short)((DataBuffer[0]<<8)+DataBuffer[1]); //讀取X軸加速度
Angle_ax = (Accel_x +220) /16384; //去除零點(diǎn)偏移,計(jì)算得到角度（弧度）
Angle_ax = Angle_ax*1.2*180/3.14; //弧度轉(zhuǎn)換為度,
/****************************角速度****************************************/
I2C_ReadBuffer(DataBuffer, GYRO_YOUT_H, 2);
Gyro_y = (short)((DataBuffer[0]<<8)+DataBuffer[1]); //靜止時(shí)角速度Y軸輸出為-18左右
Gyro_y = (Gyro_y + 18)/16.4; //去除零點(diǎn)偏移，計(jì)算角速度值
//Angle_gy = Angle_gy + Gyro_y*0.01; //角速度積分得到傾斜角度,因?yàn)榭柭?jì)算帶有時(shí)間dt，所以此處不用積分
/***************************卡爾曼融合*************************************/
Kalman_Filter(Angle_ax,Gyro_y); //卡爾曼濾波計(jì)算傾角
/****************************互補(bǔ)濾波****************************************/
/***補(bǔ)償原理是取當(dāng)前傾角和加速度獲得傾角差值進(jìn)行放大，然后與
****陀螺儀角速度疊加后再積分，從而使傾角最跟蹤為加速度獲得的
****角度 0.5為放大倍數(shù)，可調(diào)節(jié)補(bǔ)償度；0.01為系統(tǒng)周期10ms
*************************************************************/
// Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01); //因?yàn)榧踊虿患哟苏Z(yǔ)句處理效果一樣，故省略，原因搞不清楚。。。
}
void Speed_Calculate(void)
{
speed_mr=TIM_GetCounter(TIM2)-0x7fff;
speed_ml=TIM_GetCounter(TIM3)-0x7fff; //讀取編碼器寄存器計(jì)數(shù)值,并減去中間值,得到速度矢量
TIM_SetCounter(TIM2, 0x7fff);
TIM_SetCounter(TIM3, 0x7fff); //重置編碼器計(jì)數(shù)值
speed_r_l =(speed_mr + speed_ml)*0.5;
speed *= 0.7; //車輪速度濾波
speed += speed_r_l*0.3;
}
void PWM_Calculate(void)
{
if(Angle<-40||Angle>40) //傾角過(guò)大就關(guān)閉電機(jī)，進(jìn)入死循環(huán)，直到復(fù)位
{
Motor_Stop();
while(1);
}
PWM = (short)(Kp*Angle + Kd*Gyro_y); //PID：角速度和角度調(diào)節(jié)
PWM += Ksp*speed; //PID：車速度調(diào)節(jié)
}

復(fù)制代碼