四軸四元數的51單片機源程序

ID:184171 · 發表于 2017-5-16 20:14

分享個四軸四元數的51單片機代碼：

單片機源程序如下:

//四川綿陽西南科技大學信息工程學院電氣13級劉其民
//硬件參數：
//電池:1S/3.7V電池推薦300-500mAh左右
//電機/槳:720空心杯/59MM槳
//MCU IAP15W4K61S4@28.000MHZ
//陀螺儀加速度計：MPU-6050
//磁場傳感計:HMC5883L
//無線芯片:NRF24L01
//電機驅動MOS管:AO3400
//MOS管保護用肖特基:BAT54ST
//下載口保護用瞬態抑制二極管:RCLAMP0524P
//機架尺寸:94mm*94mm
//標準PID控制公式:PID=P*e(n)+I*[(e(n)+e(n-1)+...+e(0)]+D*[e(n)-e(n-1)]
//數據定義說明：
//data 51單片機片內RAM最前面128字節RAM 用ACC讀寫，速度最快
//idata 片內RAM最前面256字節的RAM 包括data 用類似指針模式訪問適合用于指針操作
//pdata 外部擴展RAM的前256字節的RAM
//xdata 外部擴展RAM 用DPTR訪問
#include <STC15W4K60S4.H>
#include <intrins.h>
#include <NRF24L01.H>
#include <MPU6050.H>
#include <math.h>
#include <STC15W4KPWM.H>
void Time0_Init(); //定時器初始化
void update(); //陀螺儀矯正
void Kalman_Filter(float Accel,float Gyro); //X軸卡爾曼濾波
void Kalman_Filter_Y(float Accel,float Gyro); //Y軸卡爾曼濾波
//*************************************自整定PID相關參數************************************************
//unsigned char xdata ERRORPID[512]={0};
//unsigned char xdata ZSYPID[100]={0};
//unsigned long int xdata ALLERROR[100]={0};
//unsigned long int TE=0,TZA=0,ADD=0,THEEND=0,LESTERROR=0;
//float BESTPID=0;
//******************************************************************************************************
unsigned char PID=0; //無線串口PID調整延時用
int ich1=0,ich2=0,ich3=0,ich4=0; //無線串口待發送數據
int speed0=0,speed1=0,speed2=0,speed3=0,V=0; //電機控制參數
int PWM0=0,PWM1=0,PWM2=0,PWM3=0; //電機控制參數
unsigned char TxBuf[20]={0};
unsigned char RxBuf[20]={0};
double g_x=0,g_y=0,g_z=0; //陀螺儀靜差消除參數
double PID_x=0,PID_y=0,PID_z=0; //每根軸PID控制量輸出
//******角度參數****************************************************************************************
double Angle_ax=0,Angle_ay=0,Angle_az=0; //由加速度計算的傾斜角度
double Angle_gy=0,Angle_gx=0,Angle_gz=0; //由角速度計算的傾斜角度
double AngleAx=0,AngleAy=0; //反三角函數處理后的角度值
double Angle=0,Angley=0; //最終傾斜角度
double Anglelate=0,Anglelatey=0,Anglezlate=0;//存儲上一次角度數據
double Ax=0,Ay=0; //加入遙控器控制量后的角度
float idata dt=0.008; //系統周期
//******卡爾曼濾波參數-X軸******************************************************************************
float data Q_angle=0.001; //對加速度計的信任度
float data Q_gyro=0.003; //對陀螺儀的信任度
float data R_angle=0.5;
char data C_0 = 1;
float data Q_bias, Angle_err;
float data PCt_0, PCt_1, E;
float data K_0, K_1, t_0, t_1;
float data Pdot[4] ={0,0,0,0};
float data PP[2][2] = { { 1, 0 },{ 0, 1 } };
void Kalman_Filter(float Accel,float Gyro)
{
Angle+=(Gyro - Q_bias) * dt; //陀螺儀積分角度（測量值-陀螺儀偏差)*dt
//Angle相當于是系統的預測值
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // 先驗估計誤差協方差的微分
Pdot[1]=- PP[1][1];
Pdot[2]=- PP[1][1];
Pdot[3]= Q_gyro;
PP[0][0] += Pdot[0] * dt; // 先驗估計誤差協方差微分的積分
PP[0][1] += Pdot[1] * dt;
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle;//估計值與預測值之間的偏差
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;//卡爾曼增益1 用于計算最優估計值
K_1 = PCt_1 / E;//卡爾曼增益2 用于計算最后估計的偏差
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;//根據卡爾曼增益1算出最優角度
Q_bias += K_1 * Angle_err;//根據卡爾曼增益2算出預測值偏差
}
//******卡爾曼濾波參數-Y軸***************************************************************************
float data Q_angley=0.001;
float data Q_gyroy=0.003;
float data R_angley=0.5;
char data C_0y = 1;
float idata Q_biasy, Angle_erry;
float idata PCt_0y, PCt_1y, Ey;
float idata K_0y, K_1y, t_0y, t_1y;
float idata Pdoty[4] ={0,0,0,0};
float idata PPy[2][2] = { { 1, 0 },{ 0, 1 } };
void Kalman_Filter_Y(float Accely,float Gyroy)
{
Angley+=(Gyroy - Q_biasy) * dt;
Pdoty[0]=Q_angley - PPy[0][1] - PPy[1][0];
Pdoty[1]=- PPy[1][1];
Pdoty[2]=- PPy[1][1];
Pdoty[3]=Q_gyroy;
PPy[0][0] += Pdoty[0] * dt;
PPy[0][1] += Pdoty[1] * dt;
PPy[1][0] += Pdoty[2] * dt;
PPy[1][1] += Pdoty[3] * dt;
Angle_erry = Accely - Angley;
PCt_0y = C_0y * PPy[0][0];
PCt_1y = C_0y * PPy[1][0];
Ey = R_angley + C_0y * PCt_0y;
K_0y = PCt_0y / Ey;
K_1y = PCt_1y / Ey;
t_0y = PCt_0y;
t_1y = C_0y * PPy[0][1];
PPy[0][0] -= K_0y * t_0y;
PPy[0][1] -= K_0y * t_1y;
PPy[1][0] -= K_1y * t_0y;
PPy[1][1] -= K_1y * t_1y;
Angley += K_0y * Angle_erry;
Q_biasy += K_1y * Angle_erry;
}
//************姿態處理和PID**************************************************************************
float idata YP=4.5,YD=115.0,YI=0.05,ERRORX;
float idata XP=4.5,XD=115.0,XI=0.05,ERRORY;
float idata ZP=4.0,ZD=2.0;
void Angle_Calcu(void)
{
Angle_ax=(GetData(ACCEL_XOUT_H))/8192; //加速度處理
Angle_az=(GetData(ACCEL_ZOUT_H))/8192; //加速度量程 +-4g/S
Angle_ay=(GetData(ACCEL_YOUT_H))/8192; //轉換關系8192LSB/g
Angle_gx=(GetData(GYRO_XOUT_H)-g_x)/65.5; //陀螺儀處理
Angle_gy=(GetData(GYRO_YOUT_H)-g_y)/65.5; //陀螺儀量程 +-500度/S
Angle_gz=(GetData(GYRO_ZOUT_H)-g_z)/65.5; //轉換關系65.5LSB/度
//**************X軸指向*******************************************************************************
AngleAx=atan(Angle_ax/sqrt(Angle_ay*Angle_ay+Angle_az*Angle_az))*180/3.141592657;
Anglelate=Ax;
//Angle=0.95*(Angle-Angle_gy*dt)+0.05*AngleAx;//一階互補濾波這邊是-Angle_gy
Kalman_Filter(AngleAx,-Angle_gy); //卡爾曼濾波
Ax=Angle+((float)RxBuf[1]-128)/7;
ERRORX+=3*Ax; //加強靜差積分強度但是不改變輸出控制量的范圍
if(ERRORX>1000){ERRORX=1000;}if(ERRORX<-1000){ERRORX=-1000;}
PID_y=Ax*XP+ERRORX*XI+(Ax-Anglelate)*XD;
speed0=0+PID_y,speed2=0-PID_y;
//**************Y軸指向*******************************************************************************
AngleAy=atan(Angle_ay/sqrt(Angle_ax*Angle_ax+Angle_az*Angle_az))*180/3.141592657;
Anglelatey=Ay;
//Angley=0.95*(Angley+Angle_gx*dt)+0.05*AngleAy;//一階互補濾波這邊是+Angle_gx
Kalman_Filter_Y(AngleAy,Angle_gx); //卡爾曼濾波
Ay=Angley+((float)RxBuf[2]-128)/7-RxBuf[4]/20; //由于結構問題，油門越大，Y軸加速度計輸出偏差越大，RxBuf[4]補償之
ERRORY+=3*Ay; //加強靜差積分強度但是不改變輸出控制量的范圍
if(ERRORY>1000){ERRORY=1000;}if(ERRORY<-1000){ERRORY=-1000;}
PID_x=Ay*YP+ERRORY*YI+(Ay-Anglelatey)*YD;
speed3=0+PID_x,speed1=0-PID_x;
//**************Z軸指向*******************************************************************************
Angle_gz+=(RxBuf[3]-128)/10;
PID_z=(Angle_gz)*ZP+(Angle_gz-Anglezlate)*ZD;
Anglezlate=Angle_gz;
speed0=speed0+PID_z,speed2=speed2+PID_z;
speed1=speed1-PID_z,speed3=speed3-PID_z;
//*****************無線串口相關***********************************************************************
ich1=Ax;
ich2=Ay;
ich3=AngleAy;
ich4=XD;
//**************速度更新******************************************************************************
if((1000-RxBuf[4]*4+speed0)>1000)PWM0=1000;
else if((1000-RxBuf[4]*4+speed0)<0)PWM0=0;
else PWM0=(1000-RxBuf[4]*4+speed0);
if((1000-RxBuf[4]*4+speed1)>1000)PWM1=1000;
else if((1000-RxBuf[4]*4+speed1)<0)PWM1=0;
else PWM1=(1000-RxBuf[4]*4+speed1);
if((1000-RxBuf[4]*4+speed2)>1000)PWM2=1000;
else if((1000-RxBuf[4]*4+speed2)<0)PWM2=0;
else PWM2=(1000-RxBuf[4]*4+speed2);
if((1000-RxBuf[4]*4+speed3)>1000)PWM3=1000;
else if((1000-RxBuf[4]*4+speed3)<0)PWM3=0;
else PWM3=(1000-RxBuf[4]*4+speed3);
if(RxBuf[4]>=10)
{PWM(PWM1,PWM2,PWM0,PWM3);}//1203
else
{PWM(1000,1000,1000,1000);}
//******************************************以下注釋內容為PID中D的自動整定模式********************************************
/*
if(RxBuf[4]>60&&THEEND==0)
{
if(Ax<0&&Ay>0)
{
ERRORPID[TE]=-Ax+Ay;
}
else if(Ax>0&&Ay<0)
{
ERRORPID[TE]=Ax-Ay;
}
else if(Ax<0&&Ay<0)
{
ERRORPID[TE]=-Ax-Ay;
}
else
{
ERRORPID[TE]=Ax+Ay;
}
TE++;
if(TE==511)
{
TE=0;
ZSYPID[TZA]=XD;
XD++;YD++;
for(ADD=0;ADD<=510;ADD++)
{
ALLERROR[TZA]+=ERRORPID[ADD];
}
TZA++;
if(TZA==100)
{
THEEND=1;
LESTERROR=ALLERROR[1];
for(TE=1;TE<=99;TE++)
{
if(LESTERROR>=ALLERROR[TE])
{
LESTERROR=ALLERROR[TE];
BESTPID=ZSYPID[TE];
}
}
XD=YD=BESTPID;
}
}
}
*/
}
void main()
{
Delay(1000);
PWMGO();
init_NRF24L01();
InitMPU6050();
Time0_Init();
RxBuf[1]=128;
RxBuf[2]=128;
RxBuf[3]=128;
RxBuf[4]=0;
//SetRX_Mode();
while(1)
{
//以下為無線串口助手發送程序
TxBuf[1]=13; TxBuf[6]=13; TxBuf[11]=15; TxBuf[16]=15;
if(ich1<0) {TxBuf[1]=12; ich1=-ich1;}
if(ich2<0) {TxBuf[6]=12; ich2=-ich2;}
if(ich3<0) {TxBuf[11]=12; ich3=-ich3;}
if(ich4<0) {TxBuf[16]=12; ich4=-ich4;}
TxBuf[2]=ich1/1000; TxBuf[3]=ich1/100%10; TxBuf[4]=ich1%100/10; TxBuf[5]=ich1%10;
TxBuf[7]=ich2/1000; TxBuf[8]=ich2/100%10; TxBuf[9]=ich2%100/10; TxBuf[10]=ich2%10;
TxBuf[12]=ich3/1000; TxBuf[13]=ich3/100%10; TxBuf[14]=ich3%100/10; TxBuf[15]=ich3%10;
TxBuf[17]=ich4/1000; TxBuf[18]=ich4/100%10; TxBuf[19]=ich4%100/10; TxBuf[0]=ich4%10;
//以上為無線串口助手發送程序
nRF24L01_TxPacket(TxBuf);
……………………
…………限于本文篇幅余下代碼請從51黑下載附件…………

復制代碼

所有資料51hei提供下載:

四軸-四元數測試版.rar (197.51 KB, 下載次數: 34)

ID:99242 · 發表于 2024-10-17 01:16

完全看不懂

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

四軸四元數的51單片機源程序

相關帖子