51單片機4軸飛控
四軸-四元數-互補濾波-串級PID -STCunio的電機和槳程序：
- // STCunio
- //四川 綿陽 西南科技大學 信息工程學院 電氣13級 劉其民 技術支持
- //本人只提供有限技術支持,截止時間：2015年3月1號,過時不候，請勿打擾。
- //本人今年大二上期，由于技術有限提問時請不要超過本程序及本硬件的范圍，什么捷聯慣導技術，擴展卡爾曼，梯度下降濾波
- //就不要問了，我也不懂，最近在自學自動控制原理，還是個小渣渣，連STM32都不會，做個四軸都還是只能苦逼地用51單片機
- //本程序屬于完全開源程序，允許用于進行二次開發，但是不能做細小改動后就聲明自己擁有版權，
- //對于抄襲后又聲明自己有版權是自己開發的我只想說我QNMLGB。
- //修改本程序應注意！
- //如果將此程序優化，修改，增加功能，須開源至阿莫論壇 STC 1T單片機 版塊
- //使用本程序應遵循GNUGPL協議！
- //本程序屬于本硬件的最終版本，不會繼續升級，但是會不定期的發布修復Bug后的程序。
- //特別鳴謝在研發過程中給予支持的朋友及團體：
- // 西南科技大學 電氣09級 劉暢
- // 西南科技大學 嵌入式技術實驗室 何科君
- // 西南科技大學 航空航天學社暨空氣動力學實驗室
- //*************************************2015年2月14日 著**************************************************************
- //硬件參數：
- //電池:1S/3.7V電池 推薦300-650mAh左右 500mah以上的電池推薦安裝在背面
- //電機/槳:720空心杯/59MM槳
- // 特別聲明：本程序測試的空心杯為淘寶店鋪 深圳杰盛電機 的720空心杯 其他廠家的電機不適用于本程序，除非自行調整PID
- // 本程序測試的螺旋槳為淘寶店鋪 虎虎平價店 的59MM直徑 1mm孔徑螺旋槳 其他廠家的螺旋槳不適用于本程序，除非自行調整PID
- //MCU IAP15W4K61S4@28.000MHZ (B版！A版單片機絕對不適合！)
- //特別說明，本程序目前只適合IAP系列的單片機，非IAP單片機使用須修改EEPROM讀寫地址
- //陀螺儀加速度計：MPU-6050
- //無線芯片:NRF24L01
- //電機驅動MOS管:AO3400
- //升壓方案:BL8530
- //3.3V穩壓方案:ME6219C-33-M5G
- //下載口保護:1K電阻
- //機架尺寸:94mm*94mm
- //設計失誤的地方：
- //MOS管保護用的肖特基放錯了位置，不過完全不影響使用，這種小四軸不加肖特基保護都行。
- //軟件參數：
- //姿態解算：四元數
- //濾波：互補濾波（From 德國開源四軸）
- //PID：串級PID 外環PI,內環PID
- //數據定義說明：
- //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>
- #include <Timer.h>
- #include <EEPROM.h>
- #include <USART.h>
- #include <IMU.H>
- //******************************************************************************************************
- float XE=0,YE=0; //角度人為修正，但是四軸漂移一般是硬件造成的，故不將此值寫入EEPROM，這個只是應急使用，發現漂移應
- //連至上位機檢查電機軸是否發生彎曲，發現問題電機及時更換
- float YM=0; //油門變化速度控制，不這樣做的話快速變化油門時四軸會失速翻轉并GG
- int ich1=0,ich2=0,ich3=0,ich4=0,ich5=0,ich6=0; //無線串口/串口相關
- int speed0=0,speed1=0,speed2=0,speed3=0,V=0; //電機速度參數
- int PWM0=0,PWM1=0,PWM2=0,PWM3=0; //加載至PWM模塊的參數
- int g_x=0,g_y=0,g_z=0; //陀螺儀矯正參數
- char a_x=0,a_y=0; //角度矯正參數
- unsigned char TxBuf[20]={0};
- unsigned char RxBuf[20]={0};
- double PID_x=0,PID_y=0,PID_z=0; //PID最終輸出量
- float FR1=0,FR2=0,FR3=0; //將char數據轉存為float型
- //*****************角度參數*************************************************
- double Gyro_y=0,Gyro_x=0,Gyro_z=0; //Y軸陀螺儀數據暫存
- double Accel_x=0,Accel_y=0,Accel_z=0; //X軸加速度值暫存
- 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 Anglezlate=0; //Z軸相關
- double Ax=0,Ay=0; //加入遙控器控制量后的角度
- //****************姿態處理和PID*********************************************
- float Out_PID_X=0,Last_Angle_gx=0;//外環PI輸出量 上一次陀螺儀數據
- float Out_XP=35,Out_XI=0.01,ERRORX_Out=0;//外環P 外環I 外環誤差積分
- float In_XP=0.4,In_XI=0.01,In_XD=9,ERRORX_In=0;//內環P 內環I 內環D 內環誤差積分
- float Out_PID_Y=0,Last_Angle_gy=0;
- float Out_YP=35,Out_YI=0.01,ERRORY_Out=0;
- float In_YP=0.4,In_YI=0.01,In_YD=9,ERRORY_In=0;
- float ZP=5.0,ZD=4.0;//自旋控制的P D
- int lastR0=0,ZT=0; //上一次RxBuf[0]數據(RxBuf[0]數據在不斷變動的) 狀態標識
- int i=0;
- void Angle_Calculate() interrupt 1
- {
- //if(YM<RxBuf[4]&&(RxBuf[4]-YM)<=2){YM++;YM++;}
- //else if(YM>RxBuf[4]&&(YM-RxBuf[4])<=2){YM--;YM--;} //防止油門變化過快而失速
- //else {YM=RxBuf[4];}
- if(RxBuf[1]>FR1){FR1+=0.2;}else if(RxBuf[1]<FR1){FR1-=0.2;}
- if(RxBuf[2]>FR2){FR2+=0.2;}else if(RxBuf[2]<FR2){FR2-=0.2;}
-
- YM=(float)RxBuf[4]*3/4;
-
- if(YM>100)//如果油門大于100 即開始起飛
- {
- if(RxBuf[0]==lastR0)//如果RxBuf[0]的數據沒有收到 即矢聯
- {
- ZT++; //狀態標識+1
- if(ZT>128){ZT=128;} //狀態標識大于128即1秒沒有收到數據，失控保護
- }
- else{ZT=0;}
- }
- else{ZT=0;} //收到信號退出失控保護
- if(ZT==128){YM=101;RxBuf[1]=128;RxBuf[2]=128;} //觸發失控保護 油門為1半少一點，緩慢下降，俯仰橫滾方向舵歸中
- lastR0=RxBuf[0];
-
- i++;
- if(i==130){i=129;}
- Accel_y= GetData(ACCEL_YOUT_H); //讀取6050數據
- Accel_x= GetData(ACCEL_XOUT_H);
- Accel_z= GetData(ACCEL_ZOUT_H);
- Gyro_x = GetData(GYRO_XOUT_H)-g_x;
- Gyro_y = GetData(GYRO_YOUT_H)-g_y;
- Gyro_z = GetData(GYRO_ZOUT_H)-g_z;
- Last_Angle_gx=Angle_gx; //儲存上一次角速度數據
- Last_Angle_gy=Angle_gy;
- Angle_ax=(Accel_x)/8192; //加速度處理
- Angle_az=(Accel_z)/8192; //加速度量程 +-4g/S
- Angle_ay=(Accel_y)/8192; //轉換關系8192LSB/g
- Angle_gx=(Gyro_x)/65.5; //陀螺儀處理
- Angle_gy=(Gyro_y)/65.5; //陀螺儀量程 +-500度/S
- Angle_gz=(Gyro_z)/65.5; //轉換關系65.5LSB/度
- //***********************************四元數解算***********************************
- IMUupdate(Angle_gx*0.0174533,Angle_gy*0.0174533,Angle_gz*0.0174533,Angle_ax,Angle_ay,Angle_az);
- //0.174533為PI/180 目的是將角度轉弧度
- //********************三角函數直接解算以供比較四元數解算精準度********************
- AngleAx=atan(Angle_ax/sqrt(Angle_ay*Angle_ay+Angle_az*Angle_az))*57.2957795f; //后面的數字是180/PI 目的是弧度轉角度
- AngleAy=atan(Angle_ay/sqrt(Angle_ax*Angle_ax+Angle_az*Angle_az))*57.2957795f;
- //**************X軸指向***********************************************************
- FR1=((float)RxBuf[1]-128)/7; //char類型轉存為float以便除法運算
-
- Ax=Angle-FR1-a_x; //角度控制量加載至角度
-
- if(YM>20)
- {
- ERRORX_Out+=Ax;//外環積分(油門小于某個值時不積分)
- }
- else
- {
- ERRORX_Out=0; //油門小于定值時清除積分值
- }
- if(ERRORX_Out>500){ERRORX_Out=500;}
- else if(ERRORX_Out<-500){ERRORX_Out=-500;}//積分限幅
-
- Out_PID_X=Ax*Out_XP+ERRORX_Out*Out_XI;//外環PI
-
- if(YM>20)
- {
- ERRORX_In+=(Angle_gy-Out_PID_X); //內環積分(油門小于某個值時不積分)
- }
- else
- {
- ERRORX_In=0; //油門小于定值時清除積分值
- }
- if(ERRORX_In>500){ERRORX_In=500;}
- else if(ERRORX_In<-500){ERRORX_In=-500;}//積分限幅
-
- PID_x=(Angle_gy+Out_PID_X)*In_XP+ERRORX_In*In_XI+(Angle_gy-Last_Angle_gy)*In_XD;//內環PID
-
- if(PID_x>1000){PID_x=1000;} //輸出量限幅
- if(PID_x<-1000){PID_x=-1000;}
-
- speed0=0-PID_x,speed2=0+PID_x;
- //**************Y軸指向**************************************************
- if(RxBuf[2]>=143||RxBuf[2]<=113);else{RxBuf[2]=128;}
- FR2=((float)RxBuf[2]-128)/7; //char類型轉存為float以便除法運算
- Ay=Angley+FR2-a_y; //角度控制量加載至角度
-
- if(YM>20)
- {
- ERRORY_Out+=Ay;//外環積分(油門小于某個值時不積分)
- }
- else
- {
- ERRORY_Out=0; //油門小于定值時清除積分值
- }
- if(ERRORY_Out>500){ERRORY_Out=500;}
- else if(ERRORY_Out<-500){ERRORY_Out=-500;}//積分限幅
-
- Out_PID_Y=Ay*Out_YP+ERRORY_Out*Out_YI;//外環PI
-
- if(YM>20)
- {
- ERRORY_In+=(Angle_gx-Out_PID_Y); //內環積分(油門小于某個值時不積分)
- }
- else
- {
- ERRORY_In=0; //油門小于定值時清除積分值
- }
- if(ERRORY_In>500){ERRORY_In=500;}
- else if(ERRORY_In<-500){ERRORY_In=-500;}//積分限幅
-
- PID_y=(Angle_gx+Out_PID_Y)*In_YP+ERRORY_In*In_YI+(Angle_gx-Last_Angle_gx)*In_YD;//內環PID
-
- if(PID_y>1000){PID_y=1000;} //輸出量限幅
- if(PID_y<-1000){PID_y=-1000;}
-
- speed3=0+PID_y,speed1=0-PID_y;//加載到速度參數
- //**************Z軸指向(Z軸隨便啦，自旋控制沒必要上串級PID)*****************************
- FR3=((float)RxBuf[3]-128)*1.5;
- Angle_gz-=FR3;
- 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=AngleAx; //此處可發送6個數據至上位機，需要發送什么數據在此處修改即可
- ich4=AngleAy;
- ich5=0;
- ich6=0;
- //**************將速度參數加載至PWM模塊*************************************************
- PWM0=(1000-YM*4+speed0);
- if(PWM0>1000){PWM0=1000;} //速度參數控制，防止超過PWM參數范圍0-1000
- else if(PWM0<0){PWM0=0;}
- PWM1=(1000-YM*4+speed1);
- if(PWM1>1000){PWM1=1000;}
- else if(PWM1<0){PWM1=0;}
- PWM2=(1000-YM*4+speed2);
- if(PWM2>1000){PWM2=1000;}
- else if(PWM2<0){PWM2=0;}
-
- PWM3=(1000-YM*4+speed3);
- if(PWM3>1000){PWM3=1000;}
- else if(PWM3<0){PWM3=0;}
- if(YM>=10)
- {PWM(PWM1,PWM2,PWM0,PWM3);}//1203
- else
- {PWM(1000,1000,1000,1000);}
- }
- void main()
- {
- PWMGO();//初始化PWM
- IAPRead();//讀取陀螺儀靜差
- InitMPU6050();//初始化MPU-6050
- Usart_Init();//初始化串口
- Time0_Init();//初始化定時器
- RxBuf[1]=128;
- RxBuf[2]=128;
- RxBuf[3]=128;
- RxBuf[4]=0;
- while(1)
- {
- Delay(500);
- nRF24L01_RxPacket(RxBuf);
- if(RxBuf[5]==1&&i>128)
- {
- IAP_Gyro();
- RxBuf[5]=0;
- EA=0;
- PWMCKS=0x10;
- T2L = 0xEB;
- T2H = 0xFF;
- PWM(960,960,960,960);
- Delay(60000); //校準完畢滴一聲
- PWM(1000,1000,1000,1000);
- PWMCKS=0x00;
- EA=1;
- i=0;
- }
- if(RxBuf[6]==1&&i>128)
- {
- IAP_Angle();
- RxBuf[6]=0;
- EA=0;
- PWMCKS=0x10;
- T2L = 0xEB;
- T2H = 0xFF;
- PWM(960,960,960,960);
- Delay(60000); //校準完畢滴一聲
- PWM(1000,1000,1000,1000);
- PWMCKS=0x00;
- EA=1;
- i=0;
- }
- //Send(ich1,ich2,ich3,ich4,ich5,ich6); //串口發送數據 如需連接上位機，須取消注釋本句！！！注釋本句是為了減小遙控延時
- }
- }
所有資料下載：
IAP15W4K58S4-小四軸飛行器DIY.rar
(524.63 KB, 下載次數: 567)
2016-5-24 13:46 上傳
點擊文件名下載附件
51飛控 可以二次開發
|
[復制鏈接]
好東西
