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

 找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 3149|回復: 1
打印 上一主題 下一主題
收起左側

基于STC15W的風力擺控制系統設計 帶視頻與程序（電賽測控題）

[復制鏈接]
跳轉到指定樓層
樓主
風力擺視頻演示：


元件清單：



單片機源程序如下:
  1. //========================================================================
  2. //        城建學院電子實驗室
  3. //        修改日期2017.07.30
  4. //        版本V1.0
  5. //========================================================================
  6. //特別聲明：此程序
  7. //         
  8. //
  9. //
  10. //                                 MCU工作頻率:24MHz
  11. //=========================================================================

  13. #include <STC15F2K60S2.h>        //STC15系列通用頭文件
  14. #include <intrins.h>                //STC特殊命令符號聲明
  15. #include <MPU6050.H>                //MPU6050數字陀螺儀
  16. #include <STC15W4KPWM.H>        //單片機所有IO口初始化-PWM口初始化
  17. #include <IMU.H>                        
  18. #include <math.H>

  20. bit busy,flag=0;
  21. int ich1=0,ich2=0,ich3=0,ich4=0,ich5=0,ich6=0;                //匿名科技上位機
  22. //==================================================//
  23. //  PID算法變量
  24. //==================================================//
  25. int speed1=0,speed2=0,speed3=0,speed4=0;//電機速度參數
  26. int PWM1=0,PWM2=0,PWM3=0,PWM4=0;                //加載至PWM模塊的參數
  27. float Angle=0;
  28. char mode=1;
  29. //*****************角度參數*************************************************
  30. float Last_Angle_gx=0;                                          //外上一次陀螺儀數據
  31. float Last_Angle_gy=0;                                                  //上一次陀螺儀數據
  32. double Gyro_y=0,Gyro_x=0,Gyro_z=0;        //Y軸陀螺儀數據暫存
  33. double Accel_x=0,Accel_y=0,Accel_z=0;          //X軸加速度值暫存
  34. double Angle_ax=0,Angle_ay=0,Angle_az=0;  //由加速度計算的加速度(弧度制)
  35. double Angle_gy=0,Angle_gx=0,Angle_gz=0;  //由角速度計算的角速率(角度制)
  36. float data AngleX=0,AngleY=0;                                  //四元數解算出的歐拉角
  37. double Ax=0,Ay=0;                    //加入遙控器控制量后的角度
  38. double Vx=0,Vy=0;                    //加入遙控器控制量后的角度
  39. float ierrorx=0,ierrory=0; //誤差積分 積分根據情況，目前測試情況不需要積分
  40. //****************姿態處理和PID*********************************************
  41. float FR1=0,FR2=0;            //xy軸期望角度
  42. float  data PID_Output;                                //PID最終輸出量

  44. float xdata Last_Ax=0;
  45. float xdata Last_Ay=0;
  46. float xdata Last_vx=0;
  47. float xdata Last_vy=0;


  50. float R=0.20f;//擺幅半長 （m）
  51. unsigned int time=0;
  52. float A1=0,A2=0;

  54. sbit key1=P2^0;
  55. sbit key2=P2^1;
  56. sbit key3=P2^2;
  57. sbit key4=P2^3;




  62. //==================================================//
  63. //   PID 手動微調參數值
  64. //==================================================//
  65. // P值適當調，D值可以給很大!!! P值過大系統就會蕩

  67. #define Out_XP        4.5f        
  68. #define        Out_XI  0.0f         //I根據情況，目前不需要I
  69. #define        Out_XD  2200.0f        

  71. #define        Out_YP        Out_XP
  72. #define        Out_YI        Out_XI
  73. #define        Out_YD        Out_XD

  75. #define PI 3.15459
  76. #define T 1601//單擺周期(ms) 根據單擺公式用桿長計算

  78. //--------------------------------------------------//
  79. //  PID算法飛控自平衡 函數
  80. //--------------------------------------------------//
  81. void Flight(void)interrupt 1
  82. {
  83.     time+=10;
  84.         Gyro_x = GetData(GYRO_XOUT_H);//讀出 X軸陀螺儀數據
  85.         Gyro_y = GetData(GYRO_YOUT_H);//讀出 Y軸陀螺儀數據
  86.         Gyro_z = GetData(GYRO_ZOUT_H);//讀出 Z軸陀螺儀數據        
  87.         Accel_y= GetData(ACCEL_YOUT_H);//讀出 X軸加速度數據
  88.         Accel_x= GetData(ACCEL_XOUT_H);//讀出 Y軸陀螺儀數據                  
  89.         Accel_z= GetData(ACCEL_ZOUT_H);//讀出 Z軸加速度數據
  90.         //姿態數據算法 （借鑒STC官方算法）
  91.         Last_Angle_gx=Angle_gx;   //儲存上一次角速度數據
  92.         Last_Angle_gy=Angle_gy;          //儲存上一次角速度數據
  93.         Angle_ax=(Accel_x)/8192;  //加速度處理
  94.         Angle_az=(Accel_z)/8192;  //加速度量程 +-4g/S
  95.         Angle_ay=(Accel_y)/8192;  //轉換關系   8192LSB/g
  96.         Angle_gx=(Gyro_x)/65.5+4;   //陀螺儀處理
  97.         Angle_gy=(Gyro_y)/65.5+3;   //陀螺儀量程 +-500度/S
  98.         Angle_gz=(Gyro_z)/65.5;   //轉換關系65.5LSB/度

  100. //***********************************四元數解算***********************************
  101.         IMUupdate(Angle_gx*0.0174533,Angle_gy*0.0174533,Angle_gz*0.0174533,Angle_ax,Angle_ay,Angle_az);//0.174533為PI/180 目的是將角度轉弧度

  103. //****平衡算法***********************************************************************



  107.     FR1=A1*sin(2*PI*time/T);        //X軸控制
  108.         if(mode==3) FR2=A2*cos(2*PI*time/T); //Y軸控制
  109.         else if(mode==4) FR2=A2*cos(2*PI*time/(T/2)); //Y軸控制
  110.         else   FR2=A2*sin(2*PI*time/T); //Y軸控制
  111.         

  113.         if(time >= 1601)time=0;


  116. //************** MPU6050 X軸指向 ***********************************************************
  117.     Ax=FR1-(AngleX+2);   //X軸偏差

  119.         ich1=Ax;
  120.         ich2=Ay;
  121.         ich3=0;                 
  122.         ich4=0;
  123.         ich5=0;
  124.         ich6=0;

  126.         ierrorx +=Last_Ax-Ax;

  128.     PID_Output = Ax*Out_XP +Out_XI*ierrorx+(Last_Ax-Ax)*Out_XD;        //PID

  130.         if(PID_Output >  1000)        PID_Output =  1000;  //輸出量限幅
  131.         if(PID_Output < -1000)        PID_Output = -1000;
  132.         
  133.         speed1 =0+ PID_Output;        speed3 =0- PID_Output;

  135.         Last_Ax=Ax;

  137. //**************MPU6050 Y軸指向**************************************************

  139.           Ay=FR2-(AngleY);  //Y軸偏差

  141.         ierrory +=Last_Ay-Ay;

  143.     PID_Output = Ay*Out_YP +Out_YI*ierrory+(Last_Ay-Ay)*Out_XD;        //PID

  145.         if(PID_Output >  1000)        PID_Output =  1000;  //輸出量限幅
  146.         if(PID_Output < -1000)        PID_Output = -1000;
  147.                
  148.         speed2 =0+ PID_Output;        speed4 = 0- PID_Output;
  149.                         
  150.         Last_Ay=Ay;           

  152. //************************PID控制數值***************************************************
  153. //**************將速度參數加載至PWM模塊*************************************************        
  154.         //速度參數控制，防止超過PWM參數范圍0-1000
  155.         if(speed1>1000)speed1=1000;else if(speed1<0)speed1=0;PWM1=(speed1);                // 5
  156.         if(speed2>1000)speed2=1000;else if(speed2<0)speed2=0;PWM2=(speed2);                // 3
  157.         if(speed3>1000)speed3=1000;else if(speed3<0)speed3=0;PWM3=(speed3);                // 4
  158.         if(speed4>1000)speed4=1000;else if(speed4<0)speed4=0;PWM4=(speed4);                // 2   

  160.         PWM(1000-PWM1,1000-PWM2,1000-PWM3,1000-PWM4);
  161. }
  162.          
  163. //--------------------------------------------------//
  164. //  時間延時 函數
  165. //--------------------------------------------------//
  166. void Delay(unsigned int x)
  167. {
  168.         unsigned int i,j;
  169.     for(i=0;i<x;i++)
  170.     for(j=0;j<250;j++);
  171. }
  172. //--------------------------------------------------//
  173. //  定時器0 初始化函數 V2.0
  174. //--------------------------------------------------//


  177. void Timer0Init(void)        //10毫秒@24.000MHz
  178. {
  179.         TR0 = 0;
  180.         AUXR &= 0x7F;        //定時器時鐘12T模式
  181.         TMOD &= 0xF0;        //設置定時器模式
  182.         IE  = 0x82;
  183.         TL0 = 0xE0;                //設置定時初值
  184.         TH0 = 0xB1;                //設置定時初值
  185.         TF0 = 0;                //清除TF0標志
  186.         TR0 = 1;                //定時器0開始計時

  188. }

  190. //--------------------------------------------------//
  191. //  上位機串口 初始化函數 V2.0
  192. //  波特率115200,24MHZ
  193. //--------------------------------------------------//

  195. void Usart_Init()  //波特率115200,24MHZ
  196. {
  197.     SCON = 0x50;        
  198.         AUXR |= 0x40;               
  199.         AUXR &= 0xFE;               
  200.         TMOD &= 0x0F;        
  201.         TL1 = 0xcc;               
  202.         TH1 = 0xFF;               
  203.         ET1 = 0;               
  204.         TR1 = 1;        
  205. }

  207. void SendData(unsigned char dat)
  208. {
  209.     while (busy);            
  210.      busy = 1;
  211.     SBUF = dat;               
  212. }
  213. void Send(int Ax,int Ay,int Az,int Gx,int Gy,int Gz)
  214. {
  215.         unsigned char sum=0;
  216.         ES = 1;  
  217.         SendData(0xAA);   
  218.         SendData(0xAA);   
  219.         SendData(0x02);   
  220.         SendData(12);     
  221.         SendData(Ax>>8);     
  222.         SendData(Ax);  
  223.         SendData(Ay>>8);
  224.         SendData(Ay);
  225.         SendData(Az);
  226.         SendData(Az>>8);
  227.         SendData(Gx);
  228.         SendData(Gx>>8);
  229.         SendData(Gy);
  230.         SendData(Gy>>8);
  231.         SendData(Gz);
  232.         SendData(Gz>>8);
  233.         sum+=0xAA;sum+=0xAA;sum+=0x02;sum+=12;
  234.         sum+=Ax>>8;sum+=Ax;sum+=Ay>>8;sum+=Ay;sum+=Az>>8;sum+=Az;
  235.         sum+=Gx>>8;sum+=Gx;sum+=Gy>>8;sum+=Gy;sum+=Gz>>8;sum+=Gz;
  236.         SendData(sum);  
  237.         ES = 0;   
  238. }


  241. void mode1()
  242. {        
  243.         A1=0;
  244.         A2=0;
  245. }

  247. void mode2()
  248. {        
  249.         A1=(atan(R*cos(PI/180*Angle)/0.75))* 57.2957795f;//根據擺幅算出角度A         0.75m擺長
  250.         A2=(atan(R*sin(PI/180*Angle)/0.75))* 57.2957795f;//根據擺幅算出角度A

  252.         /*if(0==key3)
  253.         {
  254.                 Delay(10);
  255.                 if(key3==0) Angle+=15;
  256.                 if(Angle>=360) Angle=0;
  257.                 while(!key1);
  258.         }
  259.         if(key4==0)
  260.         {
  261.                 Delay(10);
  262.                 if(key4==0) Angle-=15;
  263.                 if(Angle<=-360) Angle=0;
  264.                 while(!key4);
  265.         }*/
  266. }

  268. void mode3()
  269. {        
  270.         A1=(atan(R*cos(PI/180*Angle)/0.75))* 57.2957795f;//根據擺幅算出角度A         0.75m擺長
  271.         /*if(key3==0)
  272.         {
  273.                 Delay(10);
  274.                 if(key3==0) flag=~flag;
  275.                 while(!key3);
  276.         }*/
  277.         if(!flag)  A2=A1;
  278.         else      A2=0-A1;
  279. }

  281. void mode4()
  282. {        
  283.         A1=(atan(R*cos(PI/180*Angle)/0.75))* 57.2957795f;//根據擺幅算出角度A         0.75m擺長
  284.         A2=A1/2;
  285. }

  287. //--------------------------------------------------//
  288. //  程序 主函數
  289. //--------------------------------------------------//
  290. void main(void)
  291. {
  292.         PWMGO();                //初始化PWM
  293.         Usart_Init();//串口初始化
  294.         Delay(10);    // 延時 100
  295.         InitMPU6050();        //初始化MPU-6050
  296.         Delay(100);    // 延時 100
  297.         Timer0Init();        //初始化定時器        
  298.         EA = 1;
  299.         while(1)
  300.         {
  301.                 Send(ich1,ich2,ich3,ich4,ich5,ich6);  //串口發送數據                                
  302.                 Delay(1);
  303.                 /*if(key1==0)
  304. ……………………

  306. …………限于本文篇幅 余下代碼請從51黑下載附件…………
復制代碼

所有資料51hei提供下載:
風力擺桿.zip (95.8 KB, 下載次數: 42)


評分

參與人數 1黑幣 +100 收起 理由
admin + 100 共享資料的黑幣獎勵！

查看全部評分

分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏 分享淘帖 頂頂 踩踩
回復

使用道具 舉報

沙發
ID:1 發表于 2017-9-6 17:01 | 只看該作者
MPU-6050.C
  1. //========================================================================
  2. //        城建學院電子實驗室
  3. //        修改日期2017.07.30
  4. //        版本V1.0
  5. //========================================================================
  6. //特別聲明：此程序
  7. //         
  8. //
  9. //
  10. //                                 MCU工作頻率:24MHz
  11. //=========================================================================
  12. #include <STC15F2K60S2.h>        //STC15F2K60S2 STC15系列通用頭文件
  13. #include <intrins.h>
  14. #include <MPU6050.H>

  16. sbit    SCL=P3^5;                        //IIC時鐘引腳定義
  17. sbit    SDA=P3^4;                        //IIC數據引腳定義

  19. typedef unsigned char uchar;
  20. typedef unsigned char uint;       
  21. void  InitMPU6050();                //初始化MPU6050
  22. void  Delay2us();
  23. void  I2C_Start();
  24. void  I2C_Stop();
  25. bit   I2C_RecvACK();
  26. void  I2C_SendByte(uchar dat);
  27. uchar I2C_RecvByte();
  28. void  I2C_ReadPage();
  29. void  I2C_WritePage();
  30. uchar Single_ReadI2C(uchar REG_Address);                                        //讀取I2C數據
  31. void  Single_WriteI2C(uchar REG_Address,uchar REG_data);        //向I2C寫入數據
  32. //I^C時序中延時設置，具體參見各芯片的數據手冊  6050推薦最小1.3us 但是會出問題，這里延時實際1.9us左右
  33. void Delay2us()       
  34. {
  35.         unsigned char i;
  36.         i = 11;
  37.         while (--i);
  38. }
  39. //**************************************
  40. //I2C起始信號
  41. //**************************************
  42. void I2C_Start()
  43. {
  44.     SDA = 1;                    //拉高數據線
  45.     SCL = 1;                    //拉高時鐘線
  46.     Delay2us();                 //延時
  47.     SDA = 0;                    //產生下降沿
  48.     Delay2us();                 //延時
  49.     SCL = 0;                    //拉低時鐘線
  50. }
  51. //**************************************
  52. //I2C停止信號
  53. //**************************************
  54. void I2C_Stop()
  55. {
  56.     SDA = 0;                    //拉低數據線
  57.     SCL = 1;                    //拉高時鐘線
  58.     Delay2us();                 //延時
  59.     SDA = 1;                    //產生上升沿
  60.     Delay2us();                 //延時
  61. }
  62. //**************************************
  63. //I2C接收應答信號
  64. //**************************************
  65. bit I2C_RecvACK()
  66. {
  67.     SCL = 1;                    //拉高時鐘線
  68.     Delay2us();                 //延時
  69.     CY = SDA;                   //讀應答信號
  70.     SCL = 0;                    //拉低時鐘線
  71.     Delay2us();                 //延時
  72.     return CY;
  73. }
  74. //**************************************
  75. //向I2C總線發送一個字節數據
  76. //**************************************
  77. void I2C_SendByte(uchar dat)
  78. {
  79.     uchar i;
  80.     for (i=0; i<8; i++)         //8位計數器
  81.     {
  82.         dat <<= 1;              //移出數據的最高位
  83.         SDA = CY;               //送數據口
  84.         SCL = 1;                //拉高時鐘線
  85.         Delay2us();             //延時
  86.         SCL = 0;                //拉低時鐘線
  87.         Delay2us();             //延時
  88.     }
  89.     I2C_RecvACK();
  90. }
  91. //**************************************
  92. //從I2C總線接收一個字節數據
  93. //**************************************
  94. uchar I2C_RecvByte()
  95. {
  96.     uchar i;
  97.     uchar dat = 0;
  98.     SDA = 1;                    //使能內部上拉,準備讀取數據,
  99.     for (i=0; i<8; i++)         //8位計數器
  100.     {
  101.         dat <<= 1;
  102.         SCL = 1;                //拉高時鐘線
  103.         Delay2us();             //延時
  104.         dat |= SDA;             //讀數據
  105.         SCL = 0;                //拉低時鐘線
  106.         Delay2us();             //延時
  107.     }
  108.     return dat;
  109. }
  110. //**************************************
  111. //向I2C設備寫入一個字節數據
  112. //**************************************
  113. void Single_WriteI2C(uchar REG_Address,uchar REG_data)
  114. {
  115.     I2C_Start();                  //起始信號
  116.     I2C_SendByte(SlaveAddress);   //發送設備地址+寫信號
  117.     I2C_SendByte(REG_Address);    //內部寄存器地址，
  118.     I2C_SendByte(REG_data);       //內部寄存器數據，
  119.     I2C_Stop();                   //發送停止信號
  120. }
  121. //**************************************
  122. //從I2C設備讀取一個字節數據
  123. //**************************************
  124. uchar Single_ReadI2C(uchar REG_Address)
  125. {
  126.         uchar REG_data;
  127.         I2C_Start();                   //起始信號
  128.         I2C_SendByte(SlaveAddress);    //發送設備地址+寫信號
  129.         I2C_SendByte(REG_Address);     //發送存儲單元地址，從0開始
  130.         I2C_Start();                   //起始信號
  131.         I2C_SendByte(SlaveAddress+1);  //發送設備地址+讀信號
  132.         REG_data=I2C_RecvByte();       //讀出寄存器數據
  133.        
  134.         SDA = 1;                    //寫應答信號
  135.         SCL = 1;                    //拉高時鐘線
  136.         Delay2us();                 //延時
  137.         SCL = 0;                    //拉低時鐘線
  138.         Delay2us();                 //延時
  139.        
  140.         I2C_Stop();                    //停止信號
  141.         return REG_data;
  142. }

  144. //**************************************
  145. //初始化MPU6050
  146. //**************************************
  147. void InitMPU6050()
  148. {
  149.         Single_WriteI2C(PWR_MGMT_1, 0x00);        //解除休眠狀態
  150.         Single_WriteI2C(SMPLRT_DIV, 0x07);  //陀螺儀125hz
  151.         Single_WriteI2C(CONFIG, 0x04);      //21HZ濾波 延時A8.5ms G8.3ms  此處取值應相當注意，延時與系統周期相近為宜
  152.         Single_WriteI2C(GYRO_CONFIG, 0x08); //陀螺儀500度/S 65.5LSB/g
  153.         Single_WriteI2C(ACCEL_CONFIG, 0x08);//加速度+-4g  8192LSB/g
  154. }
  155. //**************************************
  156. //合成數據
  157. //**************************************
  158. int GetData(uchar REG_Address)
  159. {
  160.         char H,L;
  161.         H=Single_ReadI2C(REG_Address);
  162.         L=Single_ReadI2C(REG_Address+1);
  163.         return (H<<8)+L;   //合成數據
  164. }

  166.        
復制代碼
回復

使用道具 舉報

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規則

小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術交流QQ群281945664

Powered by 單片機教程網

快速回復 返回頂部 返回列表
主站蜘蛛池模板： 国产精品国产精品国产专区不片 | m豆传媒在线链接观看 | 亚洲一区国产 | 欧美不卡一区 | 免费毛片网站在线观看 | 美女爽到呻吟久久久久 | 成人亚洲精品久久久久软件 | 午夜精品久久久 | 超碰在线人 | a级片在线 | 亚洲一区 中文字幕 | 亚洲精品中文字幕 | 99久久精品国产毛片 | 自拍偷拍亚洲一区 | 欧美一二三 | 欧美精品一区二区三区蜜桃视频 | 欧美jizzhd精品欧美巨大免费 | 欧美精品一二区 | 精品国产乱码一区二区三区 | 欧美不卡视频一区发布 | 精品国产伦一区二区三区观看方式 | 九九激情视频 | 欧美日韩中文字幕在线播放 | 国产成人综合一区二区三区 | 亚洲欧美另类在线 | 情侣酒店偷拍一区二区在线播放 | 国产精品99一区二区 | 久久亚洲国产精品 | 91亚洲精品在线 | 91成人在线| 国产日韩欧美激情 | 国产成人精品午夜视频免费 | 国产精品av久久久久久久久久 | 欧美激情亚洲 | 91在线综合| 日韩欧美精品一区 | 亚洲综合视频一区 | 国产精品久久精品 | 爱操av | 国产精品一区一区 | 欧美成人久久 |