国产精品亚洲αv天堂无码,欧美日韩国产一中文字不卡,国产精品一区二区在线看

<em id="u6uq6"></em>

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

復制代碼

wwwxxx国产_337p日本欧洲亚洲大胆张筱雨_免费在线看成人av_日本黄色不卡视频_国产精品成熟老女人_99视频一区_亚洲精品97久久中文字幕_免费精品视频在线_亚洲色图欧美视频_欧美一区二三区