找回密碼
 立即注冊(cè)

QQ登錄

只需一步，快速開始

搜索
查看: 9550|回復(fù): 3
收起左側(cè)

51單片機(jī)小四軸飛行器DIY制作 附詳細(xì)注釋的源碼與電路圖

  [復(fù)制鏈接]
ID:185443 發(fā)表于 2018-12-30 10:08 | 顯示全部樓層 |閱讀模式
硬件參數(shù)：
//電池:1S/3.7V電池 推薦300-650mAh左右   500mah以上的電池推薦安裝在背面
//電機(jī)/槳:720空心杯/59MM槳         
//   特別聲明：本程序測(cè)試的空心杯為淘寶店鋪 深圳杰盛電機(jī) 的720空心杯 其他廠家的電機(jī)不適用于本程序，除非自行調(diào)整PID
//  本程序測(cè)試的螺旋槳為 虎虎平價(jià)店 的59MM直徑 1mm孔徑螺旋槳 其他廠家的螺旋槳不適用于本程序，除非自行調(diào)整PID
//MCU IAP15W4K61S4@28.000MHZ  (B版！A版單片機(jī)絕對(duì)不適合！)
//特別說(shuō)明，本程序目前只適合IAP系列的單片機(jī)，非IAP單片機(jī)使用須修改EEPROM讀寫地址
//陀螺儀加速度計(jì)：MPU-6050
//無(wú)線芯片:NRF24L01
//電機(jī)驅(qū)動(dòng)MOS管:AO3400
//升壓方案:BL8530
//3.3V穩(wěn)壓方案:ME6219C-33-M5G
//下載口保護(hù):1K電阻
//機(jī)架尺寸:94mm*94mm
//設(shè)計(jì)失誤的地方：
//MOS管保護(hù)用的肖特基放錯(cuò)了位置，不過(guò)完全不影響使用，這種小四軸不加肖特基保護(hù)都行。
//軟件參數(shù)：
//姿態(tài)解算：四元數(shù)
//濾波：互補(bǔ)濾波（From 德國(guó)開源四軸）
//PID：串級(jí)PID 外環(huán)PI,內(nèi)環(huán)PID

電路原理圖如下：
圖片2.png 圖片1.png

安裝圖

安裝圖

單片機(jī)源程序如下:
  1. //四川 綿陽(yáng)  西南科技大學(xué) 信息工程學(xué)院 電氣13級(jí)  
  2. //本人只提供有限技術(shù)支持,截止時(shí)間：2015年3月1號(hào),過(guò)時(shí)不候，請(qǐng)勿打擾。
  3. //本人今年大二上期，由于技術(shù)有限提問(wèn)時(shí)請(qǐng)不要超過(guò)本程序及本硬件的范圍，什么捷聯(lián)慣導(dǎo)技術(shù)，擴(kuò)展卡爾曼，梯度下降濾波
  4. //就不要問(wèn)了，我也不懂，最近在自學(xué)自動(dòng)控制原理，還是個(gè)小渣渣，連STM32都不會(huì)，做個(gè)四軸都還是只能苦逼地用51單片機(jī)
  5. //本程序?qū)儆谕耆_源程序，允許用于進(jìn)行二次開發(fā)，但是不能做細(xì)小改動(dòng)后就聲明自己擁有版權(quán)，
  6. //對(duì)于抄襲后又聲明自己有版權(quán)是自己開發(fā)的我只想說(shuō)我QNMLGB。
  7. //使用本程序應(yīng)遵循GNUGPL協(xié)議！

  9. //本程序?qū)儆诒居布淖罱K版本，不會(huì)繼續(xù)升級(jí)，但是會(huì)不定期的發(fā)布修復(fù)Bug后的程序。

  11. //特別鳴謝在研發(fā)過(guò)程中給予支持的朋友及團(tuán)體：
  12. //                                         西南科技大學(xué) 電氣09級(jí) 劉暢
  13. //                                         西南科技大學(xué) 嵌入式技術(shù)實(shí)驗(yàn)室 何科君
  14. //                                         西南科技大學(xué) 航空航天學(xué)社暨空氣動(dòng)力學(xué)實(shí)驗(yàn)室

  16. //硬件參數(shù)：
  17. //電池:1S/3.7V電池 推薦300-650mAh左右   500mah以上的電池推薦安裝在背面
  18. //電機(jī)/槳:720空心杯/59MM槳         
  19. //   特別聲明：本程序測(cè)試的空心杯為淘寶店鋪 深圳杰盛電機(jī) 的720空心杯 其他廠家的電機(jī)不適用于本程序，除非自行調(diào)整PID
  20. //             本程序測(cè)試的螺旋槳為淘寶店鋪 虎虎平價(jià)店 的59MM直徑 1mm孔徑螺旋槳 其他廠家的螺旋槳不適用于本程序，除非自行調(diào)整PID
  21. //MCU IAP15W4K61S4@28.000MHZ  (B版！A版單片機(jī)絕對(duì)不適合！)
  22. //特別說(shuō)明，本程序目前只適合IAP系列的單片機(jī)，非IAP單片機(jī)使用須修改EEPROM讀寫地址
  23. //陀螺儀加速度計(jì)：MPU-6050
  24. //無(wú)線芯片:NRF24L01
  25. //電機(jī)驅(qū)動(dòng)MOS管:AO3400
  26. //升壓方案:BL8530
  27. //3.3V穩(wěn)壓方案:ME6219C-33-M5G
  28. //下載口保護(hù):1K電阻
  29. //機(jī)架尺寸:94mm*94mm

  31. //設(shè)計(jì)失誤的地方：
  32. //MOS管保護(hù)用的肖特基放錯(cuò)了位置，不過(guò)完全不影響使用，這種小四軸不加肖特基保護(hù)都行。

  34. //軟件參數(shù)：
  35. //姿態(tài)解算：四元數(shù)
  36. //濾波：互補(bǔ)濾波（From 德國(guó)開源四軸）
  37. //PID：串級(jí)PID 外環(huán)PI,內(nèi)環(huán)PID

  39. //數(shù)據(jù)定義說(shuō)明：
  40. //data 51單片機(jī)片內(nèi)RAM最前面128字節(jié)RAM 用ACC讀寫，速度最快
  41. //idata 片內(nèi)RAM最前面256字節(jié)的RAM 包括data 用類似指針模式訪問(wèn) 適合用于指針操作
  42. //pdata 外部擴(kuò)展RAM的前256字節(jié)的RAM 不要用 會(huì)大姨媽！
  43. //xdata 外部擴(kuò)展RAM 用DPTR訪問(wèn)
  44. #include <STC15W4K60S4.H>
  45. #include <intrins.h>
  46. #include <NRF24L01.H>
  47. #include <MPU6050.H>
  48. #include <math.h>
  49. #include <STC15W4KPWM.H>
  50. #include <Timer.h>
  51. #include <EEPROM.h>
  52. #include <USART.h>
  53. #include <IMU.H>
  54. //******************************************************************************************************
  55. float XE=0,YE=0;                 //角度人為修正，但是四軸漂移一般是硬件造成的，故不將此值寫入EEPROM，這個(gè)只是應(yīng)急使用，發(fā)現(xiàn)漂移應(yīng)
  56.                                  //連至上位機(jī)檢查電機(jī)軸是否發(fā)生彎曲，發(fā)現(xiàn)問(wèn)題電機(jī)及時(shí)更換
  57. float YM=0;              //油門變化速度控制，不這樣做的話快速變化油門時(shí)四軸會(huì)失速翻轉(zhuǎn)并GG
  58. int ich1=0,ich2=0,ich3=0,ich4=0,ich5=0,ich6=0;                                 //無(wú)線串口/串口相關(guān)
  59. int speed0=0,speed1=0,speed2=0,speed3=0,V=0;           //電機(jī)速度參數(shù)
  60. int PWM0=0,PWM1=0,PWM2=0,PWM3=0;  //加載至PWM模塊的參數(shù)
  61. int g_x=0,g_y=0,g_z=0;            //陀螺儀矯正參數(shù)
  62. char a_x=0,a_y=0;                 //角度矯正參數(shù)
  63. unsigned char TxBuf[20]={0};
  64. unsigned char RxBuf[20]={0};  
  65. double PID_x=0,PID_y=0,PID_z=0;  //PID最終輸出量
  66. float FR1=0,FR2=0,FR3=0;         //將char數(shù)據(jù)轉(zhuǎn)存為float型
  67. //*****************角度參數(shù)*************************************************
  68. double Gyro_y=0,Gyro_x=0,Gyro_z=0;        //Y軸陀螺儀數(shù)據(jù)暫存
  69. double Accel_x=0,Accel_y=0,Accel_z=0;            //X軸加速度值暫存
  70. double Angle_ax=0,Angle_ay=0,Angle_az=0;  //由加速度計(jì)算的加速度(弧度制)
  71. double Angle_gy=0,Angle_gx=0,Angle_gz=0;  //由角速度計(jì)算的角速率(角度制)
  72. double AngleAx=0,AngleAy=0;               //三角函數(shù)解算出的歐拉角
  73. double Angle=0,Angley=0;                  //四元數(shù)解算出的歐拉角
  74. double Anglezlate=0;                      //Z軸相關(guān)
  75. double Ax=0,Ay=0;                         //加入遙控器控制量后的角度
  76. //****************姿態(tài)處理和PID*********************************************
  77. float Out_PID_X=0,Last_Angle_gx=0;//外環(huán)PI輸出量  上一次陀螺儀數(shù)據(jù)
  78. float Out_XP=35,Out_XI=0.01,ERRORX_Out=0;//外環(huán)P  外環(huán)I  外環(huán)誤差積分
  79. float In_XP=0.4,In_XI=0.01,In_XD=9,ERRORX_In=0;//內(nèi)環(huán)P  內(nèi)環(huán)I   內(nèi)環(huán)D  內(nèi)環(huán)誤差積分

  81. float Out_PID_Y=0,Last_Angle_gy=0;
  82. float Out_YP=35,Out_YI=0.01,ERRORY_Out=0;
  83. float In_YP=0.4,In_YI=0.01,In_YD=9,ERRORY_In=0;

  85. float ZP=5.0,ZD=4.0;//自旋控制的P D

  87. int lastR0=0,ZT=0; //上一次RxBuf[0]數(shù)據(jù)(RxBuf[0]數(shù)據(jù)在不斷變動(dòng)的)   狀態(tài)標(biāo)識(shí)
  88. int i=0;
  89. void Angle_Calculate() interrupt 1
  90. {        
  91.         //if(YM<RxBuf[4]&&(RxBuf[4]-YM)<=2){YM++;YM++;}
  92.         //else if(YM>RxBuf[4]&&(YM-RxBuf[4])<=2){YM--;YM--;}  //防止油門變化過(guò)快而失速
  93.         //else {YM=RxBuf[4];}
  94.         if(RxBuf[1]>FR1){FR1+=0.2;}else if(RxBuf[1]<FR1){FR1-=0.2;}
  95.         if(RxBuf[2]>FR2){FR2+=0.2;}else if(RxBuf[2]<FR2){FR2-=0.2;}
  96.         
  97.         YM=(float)RxBuf[4]*3/4;
  98.         
  99.         if(YM>100)//如果油門大于100 即開始起飛
  100.         {
  101.                 if(RxBuf[0]==lastR0)//如果RxBuf[0]的數(shù)據(jù)沒有收到 即矢聯(lián)
  102.                 {
  103.                         ZT++;  //狀態(tài)標(biāo)識(shí)+1
  104.                         if(ZT>128){ZT=128;}  //狀態(tài)標(biāo)識(shí)大于128即1秒沒有收到數(shù)據(jù)，失控保護(hù)
  105.                 }
  106.                 else{ZT=0;}
  107.         }
  108.         else{ZT=0;} //收到信號(hào)退出失控保護(hù)
  109.         if(ZT==128){YM=101;RxBuf[1]=128;RxBuf[2]=128;} //觸發(fā)失控保護(hù) 油門為1半少一點(diǎn)，緩慢下降，俯仰橫滾方向舵歸中
  110.         lastR0=RxBuf[0];
  111.         
  112.         i++;
  113.         if(i==130){i=129;}
  114.         Accel_y= GetData(ACCEL_YOUT_H);        //讀取6050數(shù)據(jù)
  115.         Accel_x= GetData(ACCEL_XOUT_H);                  
  116.         Accel_z= GetData(ACCEL_ZOUT_H);            
  117.         Gyro_x = GetData(GYRO_XOUT_H)-g_x;
  118.         Gyro_y = GetData(GYRO_YOUT_H)-g_y;
  119.         Gyro_z = GetData(GYRO_ZOUT_H)-g_z;        
  120.         Last_Angle_gx=Angle_gx;   //儲(chǔ)存上一次角速度數(shù)據(jù)
  121.         Last_Angle_gy=Angle_gy;        
  122.         Angle_ax=(Accel_x)/8192;  //加速度處理
  123.         Angle_az=(Accel_z)/8192;  //加速度量程 +-4g/S
  124.         Angle_ay=(Accel_y)/8192;        //轉(zhuǎn)換關(guān)系8192LSB/g
  125.         Angle_gx=(Gyro_x)/65.5;   //陀螺儀處理
  126.         Angle_gy=(Gyro_y)/65.5;   //陀螺儀量程 +-500度/S
  127.         Angle_gz=(Gyro_z)/65.5;   //轉(zhuǎn)換關(guān)系65.5LSB/度
  128. //***********************************四元數(shù)解算***********************************
  129.         IMUupdate(Angle_gx*0.0174533,Angle_gy*0.0174533,Angle_gz*0.0174533,Angle_ax,Angle_ay,Angle_az);
  130.         //0.174533為PI/180 目的是將角度轉(zhuǎn)弧度
  131. //********************三角函數(shù)直接解算以供比較四元數(shù)解算精準(zhǔn)度********************
  132.         AngleAx=atan(Angle_ax/sqrt(Angle_ay*Angle_ay+Angle_az*Angle_az))*57.2957795f; //后面的數(shù)字是180/PI 目的是弧度轉(zhuǎn)角度
  133.   AngleAy=atan(Angle_ay/sqrt(Angle_ax*Angle_ax+Angle_az*Angle_az))*57.2957795f;
  134. //**************X軸指向***********************************************************
  135.         FR1=((float)RxBuf[1]-128)/7; //char類型轉(zhuǎn)存為float以便除法運(yùn)算
  136.         
  137.         Ax=Angle-FR1-a_x;      //角度控制量加載至角度
  138.         
  139.         if(YM>20)
  140.         {
  141.   ERRORX_Out+=Ax;//外環(huán)積分(油門小于某個(gè)值時(shí)不積分)
  142.         }
  143.         else
  144.         {
  145.                 ERRORX_Out=0; //油門小于定值時(shí)清除積分值
  146.         }
  147.         if(ERRORX_Out>500){ERRORX_Out=500;}
  148.         else if(ERRORX_Out<-500){ERRORX_Out=-500;}//積分限幅
  149.         
  150.         Out_PID_X=Ax*Out_XP+ERRORX_Out*Out_XI;//外環(huán)PI
  151.         
  152.         if(YM>20)
  153.         {
  154.   ERRORX_In+=(Angle_gy-Out_PID_X);  //內(nèi)環(huán)積分(油門小于某個(gè)值時(shí)不積分)
  155.         }        
  156.   else
  157.         {
  158.                 ERRORX_In=0; //油門小于定值時(shí)清除積分值
  159.         }        
  160.         if(ERRORX_In>500){ERRORX_In=500;}
  161.   else if(ERRORX_In<-500){ERRORX_In=-500;}//積分限幅
  162.         
  163.         PID_x=(Angle_gy+Out_PID_X)*In_XP+ERRORX_In*In_XI+(Angle_gy-Last_Angle_gy)*In_XD;//內(nèi)環(huán)PID
  164.         
  165.   if(PID_x>1000){PID_x=1000;}  //輸出量限幅
  166.         if(PID_x<-1000){PID_x=-1000;}
  167.         
  168.         speed0=0-PID_x,speed2=0+PID_x;
  169. //**************Y軸指向**************************************************
  170.         if(RxBuf[2]>=143||RxBuf[2]<=113);else{RxBuf[2]=128;}
  171.         FR2=((float)RxBuf[2]-128)/7; //char類型轉(zhuǎn)存為float以便除法運(yùn)算
  172.         Ay=Angley+FR2-a_y;      //角度控制量加載至角度
  173.         
  174.         if(YM>20)
  175.         {
  176.   ERRORY_Out+=Ay;//外環(huán)積分(油門小于某個(gè)值時(shí)不積分)
  177.         }
  178.         else
  179.         {
  180.                 ERRORY_Out=0; //油門小于定值時(shí)清除積分值
  181.         }        
  182.         if(ERRORY_Out>500){ERRORY_Out=500;}
  183.         else if(ERRORY_Out<-500){ERRORY_Out=-500;}//積分限幅
  184.         
  185.         Out_PID_Y=Ay*Out_YP+ERRORY_Out*Out_YI;//外環(huán)PI
  186.         
  187.         if(YM>20)
  188.         {
  189.   ERRORY_In+=(Angle_gx-Out_PID_Y);  //內(nèi)環(huán)積分(油門小于某個(gè)值時(shí)不積分)
  190.         }               
  191.         else
  192.         {
  193.                 ERRORY_In=0; //油門小于定值時(shí)清除積分值
  194.         }        
  195.         if(ERRORY_In>500){ERRORY_In=500;}
  196.   else if(ERRORY_In<-500){ERRORY_In=-500;}//積分限幅
  197.         
  198.         PID_y=(Angle_gx+Out_PID_Y)*In_YP+ERRORY_In*In_YI+(Angle_gx-Last_Angle_gx)*In_YD;//內(nèi)環(huán)PID
  199.         
  200.   if(PID_y>1000){PID_y=1000;}  //輸出量限幅
  201.         if(PID_y<-1000){PID_y=-1000;}
  202.         
  203.         speed3=0+PID_y,speed1=0-PID_y;//加載到速度參數(shù)
  204. //**************Z軸指向(Z軸隨便啦，自旋控制沒必要上串級(jí)PID)*****************************        
  205.         FR3=((float)RxBuf[3]-128)*1.5;
  206.         Angle_gz-=FR3;
  207.         PID_z=(Angle_gz)*ZP+(Angle_gz-Anglezlate)*ZD;
  208.         Anglezlate=Angle_gz;
  209.         speed0=speed0+PID_z,speed2=speed2+PID_z;
  210.         speed1=speed1-PID_z,speed3=speed3-PID_z;
  211. //*****************串口及無(wú)線串口相關(guān)***************************************************
  212.         ich1=Ax;
  213.         ich2=Ay;
  214.         ich3=AngleAx;                 //此處可發(fā)送6個(gè)數(shù)據(jù)至上位機(jī)，需要發(fā)送什么數(shù)據(jù)在此處修改即可
  215.         ich4=AngleAy;
  216.         ich5=0;
  217.         ich6=0;
  218. //**************將速度參數(shù)加載至PWM模塊*************************************************        
  219.         PWM0=(1000-YM*4+speed0);
  220.         if(PWM0>1000){PWM0=1000;}    //速度參數(shù)控制，防止超過(guò)PWM參數(shù)范圍0-1000
  221.         else if(PWM0<0){PWM0=0;}

  223.         PWM1=(1000-YM*4+speed1);
  224.         if(PWM1>1000){PWM1=1000;}
  225.         else if(PWM1<0){PWM1=0;}

  227.         PWM2=(1000-YM*4+speed2);
  228.         if(PWM2>1000){PWM2=1000;}
  229.         else if(PWM2<0){PWM2=0;}
  230.         
  231.         PWM3=(1000-YM*4+speed3);
  232.         if(PWM3>1000){PWM3=1000;}
  233.         else if(PWM3<0){PWM3=0;}
  234.   if(YM>=10)
  235.         {PWM(PWM1,PWM2,PWM0,PWM3);}//1203
  236.         else
  237.         {PWM(1000,1000,1000,1000);}

  239. }
  240. void main()
  241. {
  242. PWMGO();//初始化PWM
  243. IAPRead();//讀取陀螺儀靜差
  244. InitMPU6050();//初始化MPU-6050
  245. Usart_Init();//初始化串口
  246. Time0_Init();//初始化定時(shí)器
  247. RxBuf[1]=128;
  248. RxBuf[2]=128;
  249. RxBuf[3]=128;
  250. RxBuf[4]=0;
  251. while(1)
  252. {           
  253.                                 Delay(500);
  254.                                 nRF24L01_RxPacket(RxBuf);
  255.                                 if(RxBuf[5]==1&&i>128)        
  256.         {
  257.                                 IAP_Gyro();
  258.                                 RxBuf[5]=0;
  259.                                 EA=0;
  260.                                 PWMCKS=0x10;         
  261.                                 T2L = 0xEB;        
  262.               T2H = 0xFF;
  263.                                 PWM(960,960,960,960);
  264.               Delay(60000); //校準(zhǔn)完畢滴一聲
  265.         PWM(1000,1000,1000,1000);        
  266.         PWMCKS=0x00;
  267.         EA=1;
  268.         i=0;                                       
  269.                                 }
  270.                                 if(RxBuf[6]==1&&i>128)        
  271.         {
  272.                                 IAP_Angle();
  273.                                 RxBuf[6]=0;
  274.                                 EA=0;
  275.                                 PWMCKS=0x10;         
  276.                                 T2L = 0xEB;        
  277.               T2H = 0xFF;
  278.                                 PWM(960,960,960,960);
  279.               Delay(60000);  //校準(zhǔn)完畢滴一聲
  280.         PWM(1000,1000,1000,1000);        
  281.         PWMCKS=0x00;
  282.                                 EA=1;        
  283.                                 i=0;
  284.                                 }
  285.               //Send(ich1,ich2,ich3,ich4,ich5,ich6);  //串口發(fā)送數(shù)據(jù)  如需連接上位機(jī)，須取消注釋本句�。�！注釋本句是為了減小遙控延時(shí)
  286. }
  287. }
復(fù)制代碼
0.png
所有資料51hei提供下載:
51單片機(jī)-小四軸飛行器DIY-2015-3-5.rar (524.63 KB, 下載次數(shù): 205)




評(píng)分

參與人數(shù) 2黑幣 +80 收起 理由
dww465757120 + 30 共享資料的黑幣獎(jiǎng)勵(lì)！
admin + 50 共享資料的黑幣獎(jiǎng)勵(lì)！

查看全部評(píng)分

回復(fù)

使用道具 舉報(bào)

ID:487712 發(fā)表于 2019-3-10 03:30 來(lái)自手機(jī) | 顯示全部樓層
真心想要
回復(fù)

使用道具 舉報(bào)

ID:58190 發(fā)表于 2019-3-11 18:53 | 顯示全部樓層
謝謝分享。。。。。。。
回復(fù)

使用道具 舉報(bào)

ID:879330 發(fā)表于 2021-1-21 01:49 | 顯示全部樓層
準(zhǔn)備做一個(gè)....
回復(fù)

使用道具 舉報(bào)

本版積分規(guī)則

手機(jī)版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

Powered by 單片機(jī)教程網(wǎng)

快速回復(fù) 返回頂部 返回列表