久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费
標題:
STM32自平衡小車源代碼
[打印本頁]
作者:
liukong
時間:
2017-3-16 23:19
標題:
STM32自平衡小車源代碼
平衡小車代碼
0.png
(49.98 KB, 下載次數: 80)
下載附件
2017-3-16 23:44 上傳
全部源碼下載:
STM32自平衡小車源代碼.7z
(231.18 KB, 下載次數: 100)
2022-12-16 04:25 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
部分代碼預覽:
#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:車速度調節
}
復制代碼
作者:
cm7626
時間:
2017-4-24 08:35
平衡車的代碼夠復雜的 初學者就不用考慮了
作者:
ddxx
時間:
2017-12-7 12:17
沒錢下載啊。
作者:
lemon0210
時間:
2017-12-7 14:51
只有代碼嗎
作者:
wj1994
時間:
2017-12-11 13:35
有人可以說一下有原理圖嗎?
作者:
張碭碭
時間:
2018-5-17 16:41
濾波原理
歡迎光臨 (http://m.zg4o1577.cn/bbs/)
Powered by Discuz! X3.1
主站蜘蛛池模板:
日韩精品小视频
|
日韩免费在线观看视频
|
亚欧在线观看
|
日本黄a三级三级三级
|
欧美日韩在线精品
|
免费视频久久久
|
欧美精品二区三区四区免费看视频
|
国产伦精品一区二区三毛
|
国模精品视频一区二区
|
av高清在线
|
精品视频在线免费观看
|
欧美一级久久
|
青青草视频网站
|
午夜免费观看视频
|
九九热在线观看
|
亚洲一区二区在线视频
|
国产精品伦子伦免费视频
|
天天操天天碰
|
黑人精品xxx一区一二区
|
国产免费高清视频
|
欧美一级在线观看
|
久久精品欧美一区二区
|
97精品国产97久久久久久免费
|
成人欧美激情
|
国产精品婷婷
|
黄色一级大片在线免费看国产一
|
免费久久久
|
草草视频在线
|
av免费网站
|
中文字幕在线观看网址
|
91女人18毛片水多国产
|
成av人片一区二区三区久久
|
午夜看片
|
久久久久女人精品毛片九一
|
久久久久人
|
欧美日韩一区二区三区视频
|
久久激情视频
|
一本色道久久综合亚洲精品小说
|
欧美日韩在线免费
|
欧美一级做性受免费大片免费
|
一区二区不卡
|