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

 找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

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

風力擺控制系統賽題解析與操作說明及STM32源碼(重要的是PID算法)

  [復制鏈接]
跳轉到指定樓層
樓主
電子競賽題目《風力擺控制系統》
本人的課程設計；
重要的是PID算法。



沒有電路圖。

電機的控制：電機1  控制正反轉   ->PD14,PD12      PWM波 -> PA1
        電機2  控制正反轉   ->PD15,PD13      PWM波 -> PA0
            電機3  控制正反轉   ->PD11,PD10      PWM波 -> PA3
            電機4  控制正反轉   ->PD9,PD8        PWM波 -> PA2

MPU6050    SDA ->PB7     SCL ->PB6

按鍵   K1-> PA4, K2->PA5, K3->PA6,  K4->PA7.


操作：K3為開始鍵  ；K4為選擇模式；K1畫圓時增加半徑，畫直線時增加長度；K2畫圓時減小半徑，畫直線時減小長度；
      按一下K4為畫直線，按兩下是長度可控的直線，按下是角度可變的直線，按四下是5S靜止，按五下是畫圓

風力擺控制系統賽題解析
選型方案
第一步，控制系統選型：剛才說了，我們需要做一個伺服隨動控制系統，通常衡量伺服系統性能的指標有“帶寬”、“精度”、“抗干擾能力”等。先說系統帶寬，帶寬反映風力擺跟蹤的快速性。帶寬越大，快速性越好。風力擺控制系統的帶寬主要受到控制對象和執行機構的慣性的限制。慣性越大，帶寬越窄。根據題目要求做“自由擺運動”，由單擺周期公式可以求得風力擺的擺動周期 T 在 1.3-1.6 秒之間，換算成頻率 f 在 0.625-0.77Hz 之間，大家可以自己掐表計算一下擺動的周期。由此可知我們需要設計出一個帶寬大于 0.77Hz 的控制系統（取0.8Hz）即可完成題目的要求。出題的專家制定的這個指標還是簡單的，要知道現代伺服控
制系統帶寬已經超過了 50Hz。角度采樣率根據奈奎斯特采樣定理，理論上選取 fs>2f 即可但是題目中要求了系統最大調節時間，為了使得控制效果更好，需要取 fs>10f 甚至更高，在本次設計中采樣率選取 200Sa/s，控制周期 5ms。
第二步，電機選型：這是本題的最大爭議，在此我也不想把這個爭議擴大，所以我不討論電機是否違規的問題。現在從實現效果這個角度來討論電機的選型，不管黑貓白貓，抓到老鼠就是好貓！
我把軸流電機、空心杯電機、無刷電機做成一個表格。


主要看推重比和機動性這一欄，推重比、機動性是航空器的專業術語，發動機在水平面上的最大推力和發動機的凈重之比稱為推重比。機動性是指風力擺在一定時間內改變運動速度、方向的能力。從這個表格來看，推重比是我們選擇電機的重要指標，而機動性是整個風力擺靈活性的重要指標。注意到賽題目說明部分的第 9 條：“賽題中要求的各項動作完成時間越短越好”。毫無疑問，出題人已經說明了要盡可能的提高系統的機動性。從最優實現效果來看，無刷電機和空心杯電機是更優的選擇。但是為什么有人用軸流電機也能完成題目要求呢？答案就在頻率響應上，第一步已經分析了系統帶寬 0.8Hz 即可滿足題目要求。如果題目規定 15 秒內完成20 個單擺運動，這個難度就上來了，因為用軸流電機的系統帶寬不夠了。但是采用軸流電機有個好處，在圓周運動時即使做開環控制風扇干擾幾乎吹不動。第三步，傳感器選型：我們再說說精度（檢測誤差），檢測誤差包括傳感器的誤差和機械誤差，是傳感器和機械本身所固有的，控制系統無法克服。根據題目給出的±2.5cm 偏差可以知傳感器精度只要達到 0.5°就可以了，所以用 MPU6050 傳感器是可以達到題目要求的。機械誤差則要求機械完美的對稱，所有的東西都要對稱，機械做的越好，最后出來的效果就越好。至于傳感器的數據融合、濾波器設計部分請看程序貼圖：計算三次角度然后求平均值，再經過卡爾曼濾波器濾波的角度值就可以使用了。角度數據一定要穩定，無高頻干擾！

（如果圖片中的字看不清可以下載附件直接看）

算法設計
當硬件 OK 以后算法就是整個系統的核心靈魂了，俗話說條條大路通羅馬，能夠達到題目要求的方法有很多，這次比賽童鞋們用的算法大概有幾種方式：邏輯判斷法、查表法，定點PID 法，力合成法，矢量方程法等等。有的隊直接采用查表法開環控制也能取得不錯的成績，只要能夠達到要求的算法都是 OK 的。
說一下我的算法設計吧，出題人在第一問就告訴了要做自由擺運動，從自由擺可以聯想到2011 年的 B 題——基于自由擺的平板控制系統。這是一個單擺，但是單擺是非線性運動，我們通常會把非線性的東西通過某種思路去近似成線性的（如二極管的伏安特性曲線等效模型），在單擺中，當采樣率足夠高時兩個采樣點之間的連線可以近似看成是線性的,有木有感到很熟悉？在高中物理學過了簡諧運動，物體所受的力跟位移成正比，并且總是指向平衡位置。這是一種由自身系統性質決定的周期性運動。（如單擺運動和彈簧振子運動）實際上簡諧振動就是正弦振動！其數學方程為θ(t)=Asin(ωt+ψ)，A 是振幅，ω是頻率，ψ是相位。好！說到這，來直接看一下題目第二問的要求（第一問直接跳過）。題目要求幅度可控，幅度可控是啥？不就是振幅 A 可控嘛，可控范圍是多少？30-60cm 換成角度。好了，分析完了，第二問就是一個 A 可設置的正弦運動，OK。至于線性度偏差，只要你機械搭對稱了，這都不是問題，況且別忘了有 X,Y 兩個方向的電機呢。
把第二問的程序貼圖出來：


第三問，擺動方向可設置。這里需要介紹一個知識點了：李薩茹圖形。當風力擺同時參與兩個相互垂直方向的簡諧振動,風力擺的位移是這兩個振動的位移的矢量和,如果兩個振動的頻率具有整數比關系時,風力擺的運動路徑是穩定的封閉的曲線，這些曲線即李薩如圖形。具體的理論推導和計算這里就不貼出來了，可以自行百度，現在直接把結論貼出來，李薩茹圖形由以下參數方程定義：
x(θ)=Asinθ
y(θ)=Bsin(θ+ψ)
若 A=B，ψ=任意，則曲線是橢圓；


直接看圖清晰明了，第三問我們只需要在 X,Y 方向分別進行頻率相同，相位ψ=0 或π，振幅
A,B 可設置的簡諧運動即可很好的完成題目的要求。
例如，想要 45°怎么辦？tan45=1，設置振幅 A/B=1 即可：
x(θ)=10*sinθ
y(θ)=10*sinθ
例如，想要 60°怎么辦？tan60=根號 3，設置振幅 A/B=根號 3 即可：
x(θ)=根號 3*sinθ
y(θ)= 1*sinθ
例如，想要 120°怎么辦？tan120=-根號 3，設置振幅 A/B=-根號 3 即可：
x(θ)=-根號 3*sinθ
y(θ)= 1*sinθ
細心的童鞋一定會發現，出題人對第三問的要求簡單了，只要求擺動角度可控，但是通過上
述方法不僅角度可控，擺長也是可控的！

把第三問的程序貼圖出來：


第四問，5 秒內能夠制動。這個沒有什么運動路徑可言，純粹是考察大家的 PID 是否熟練掌握，PID 目標值設為 0°即可。注意 D 要給大一些，不要有積分。
第四問的程序圖：



第五問，畫圓。出題人通過基礎部分一步一步引導完成畫圓，真是用心良苦。如果能夠領
會出題人的想法，把第二問、第三問完成好，這一問是水到渠成的事情。再次把第三問的圖
貼出來，如何畫圓？
若 A=B，ψ=π/2 或 3π/2，則曲線是圓！
OK！完成了，這是圓，而且畫出來很圓！如果畫出了橢圓，一定是相位ψ有偏差，調節一下
相位即可！
第五問的程序貼圖：


細心的童鞋會發現，題目做到這一步，θ(t)=Asin(ωt+ψ)這個方程就是貫穿本題的核心！如果你的系統做得足夠好，就會發現這個方程的每一個參數你都可以自由的控制，結果就是你不僅可以讓擺幅可控、擺動周期可控、擺動方向可控，而且可以畫出以下這些曲線：




第六問：畫圓抗干擾。考察伺服系統的抗擾動能力，出題人用這一問來把開環，半開環控制和閉環控制的隊伍拉開距離。當一個干擾過來的時候，風力擺的運動方程肯定不是θ(t)=Asin(ωt+ψ)了，題目給出了系統最大調節時間為 5 秒，能夠在 5 秒內恢復 Asin(ωt+ψ)即可。這就牽扯到了伺服控制系統的重要指標——快速跟蹤和準確定位。有很多種算法可以選擇，例如常見的 LQR、自適應、模糊控制、神經網絡等等。相信絕大部分還是用了經典的 PID 算法，簡單，快速，效果尚可！就看誰的PID調得熟練了，這個需要功夫在平時，多調多想多看就 OK！PID 需要調成大概是這樣的波形，能夠在一個周期之內快速跟蹤正弦運動：


如果調成這樣，波形相似相位跟不上，系統慣性大，請把 D 調高。


第七問：其他部分，今年出題人給其他項分配了 10 分，想拿高分的隊伍是必須拿下這 10分的。前面說了，如果你的控制器做得足夠好，是能夠畫出花樣百出的圖形的，有很大的發揮余地。有的隊伍是畫三角形、正方形、畫 8 字，有的是做跟蹤物體、手寫寫字、畫斜直線，做漂亮的 GUI 界面等等都有。根據以往經驗，在有限的時間內建議不要把時間花在漂亮的GUI 界面上，要放在更多的運動控制上，牢牢把握題目考察的側重點拿分！總的來看，今年這道題目難度適中，出題人還是考慮得比較周全的，讓各種風機各種算法都能夠達到要求，眾口難調，確實不簡單。同時也反映了電賽控制類的趨勢。從 2011 年的小車這種純邏輯判斷的題目到 2013 年倒立擺這種定點穩態系統再到 2015 年風力擺的動態跟蹤系統，實現了從邏輯到算法、從靜態到動態、從二維到三維的轉變，準備 2017 年電賽控制類的同學要多多留心了。個人感覺功夫還需在平時，搞突擊是不行的，多參加比賽，多接觸實際的項目好處多多，機會留給有準備的人，相信你也可以！

stm32單片機源程序如下:

  2. #include "public.h"
  3. #include "systick.h"
  4. #include "mpu6050.h"
  5. #include "iic_analog.h"
  6. #include "indkey.h"
  7. #include "Time_Key.h"
  8. #include "Beep.h"
  9. #include "moto_PWM.h"
  10. #include "Data.h"
  11. #include "ahrs.h"
  12. #include "pwm.h"
  13. #include "usart2.h"

  15. extern uint8_t Q1_Start;
  16. extern uint8_t Q2_Start;
  17. extern uint8_t Q3_Start;
  18. extern uint8_t Q4_Start;
  19. extern uint8_t Q5_Start;
  20. extern uint8_t Q6_Start;
  21. extern uint8_t CurMode;
  22. //void gpio_init(void);

  24. void BSP_Init(void)
  25. {         
  26.   //static u8 max,max1;
  27.         
  28.         SystemInit();
  29.         pwm_init();         //PWM初始化
  30.         //gpio_init();
  31.         Key_IO_Init();
  32.         MPU6050_Init();
  33.         //delay_ms(500);
  34.         //while(MPU_Init()==0);
  35.         TIM5_Config(5000-1,71);
  36.         TIM1_Config(10000-1,71);
  37.         TIM_Cmd(TIM1,ENABLE);            
  38.         TIM_Cmd(TIM5,ENABLE);
  39.         USART2_Config();
  40.         Display_Title();
  41.         //GPIO_SetBits(GPIOB,GPIO_Pin_7);
  42.         
  43.         moto_control();
  44.         //GPIO_ResetBits(GPIOB,GPIO_Pin_7);
  45.         //moto3_turn(500,1);
  46. }
  47. int main(void)
  48.         
  49. {  
  50.         static u8 max,max1;
  51.         BSP_Init();
  52.         while(1)
  53.         {
  54.    
  55.                
  56.                 if(Q1_Start == 1)
  57.                 {
  58.                         CurMode = 1;
  59.                 }
  60.                 else if(Q2_Start == 1)
  61.                 {
  62.                         CurMode = 2;
  63.                 }
  64.                 else if(Q3_Start == 1)
  65.                 {
  66.                          CurMode = 3;
  67.                 }
  68.                 else if(Q4_Start == 1)
  69.                 {
  70.                          CurMode = 4;
  71.                 }
  72.                 else if(Q5_Start == 1)
  73.                 {
  74.                         CurMode = 5;
  75.                 }
  76.                
  77.                 else
  78.                 {
  79.                         CurMode = 0;
  80.                 }        
  81.                
  82.                 if(PID_data.erro>max)
  83.                 max=PID_data.erro;

  85.                                 if(PID_data1.erro>max1)
  86.                     max1=PID_data1.erro;
  87.                         /*if(key_board())
  88.                                 key_nuber=key_board();*/
  89.                         //delay_ms(200);
  90.                
  91.         }
  92. }




  97. //void gpio_init(void)
  98. //{
  99. //        GPIO_InitTypeDef GPIO_InitStructure;        //聲明一個結構體變量，用來初始化GPIO

  101. //        SystemInit();
  102. //        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);   /*開啟GPIO時鐘*/
  103. //        /*  配置GPIO的模式和IO口 */
  104. //        GPIO_InitStructure.GPIO_Pin=GPIO_Pin_7;                //選擇你要設置的IO口
  105. //        GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP;                  //設置推挽輸出模式
  106. //        GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;          //設置傳輸速率
  107. //        GPIO_Init(GPIOB,&GPIO_InitStructure);          /* 初始化GPIO */
  108. //}


復制代碼
  1. #include "Key.h"
  2. #include "stm32f10x.h"
  3. #include "systick.h"
  4. uint8_t Item = 0;

  6. uint8_t Q1_Start = 0;
  7. uint8_t Q2_Start = 0;
  8. uint8_t Q3_Start = 0;
  9. uint8_t Q4_Start = 0;
  10. uint8_t Q5_Start = 0;
  11. uint8_t Q6_Start = 0;
  12. uint8_t Q7_Start = 0;
  13. extern char buf[512];         //液晶屏數據緩沖區
  14. extern float R;
  15. extern float angle;
  16. extern uint8_t RoundDir;
  17. extern uint8_t CurMode;
  18. void Key_GPIO_Init(void)
  19. {
  20.         GPIO_InitTypeDef GPIO_InitStructure;        //聲明一個結構體變量，用來初始化GPIO
  21.         
  22.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);   /* 開啟GPIO時鐘 */
  23.         //RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);   /* 開啟GPIO時鐘 */
  24.         SystemInit();
  25.         /*  配置GPIO的模式和IO口 */
  26.         GPIO_InitStructure.GPIO_Pin=GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_14|GPIO_Pin_15;                //選擇你要設置的IO口
  27.         GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IPU;                  //設置推挽輸出模式
  28.         GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;          //設置傳輸速率
  29.         GPIO_Init(GPIOA,&GPIO_InitStructure);          /* 初始化GPIO */
  30.         
  31. //        GPIO_InitStructure.GPIO_Pin=GPIO_Pin_10|GPIO_Pin_11;
  32. //        GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IPU;                  //設置推挽輸出模式
  33. //        GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;          //設置傳輸速率
  34. //        GPIO_Init(GPIOB,&GPIO_InitStructure);
  35. }

  37. //u8 Key_Scan(GPIO_TypeDef* GPIO,uint16_t Pin)
  38. //{
  39. //                if(GPIO_ReadInputDataBit(GPIO,Pin)==0)
  40. //                {
  41. //                        delay_ms(20);
  42. //                        if(GPIO_ReadInputDataBit(GPIO,Pin)==0)
  43. //                        {
  44. //                                while (GPIO_ReadInputDataBit(GPIO,Pin)==0);
  45. //                                return 1;
  46. //                        }
  47. //                }
  48. //                return 0;
  49. //}


  52. //u8 key_board(void)
  53. //{
  54. //        u8 key=0;
  55. //        key=Key_Scan(GPIOA,GPIO_Pin_4)+
  56. //                        Key_Scan(GPIOA,GPIO_Pin_5)*2+
  57. //                        Key_Scan(GPIOA,GPIO_Pin_6)*3+
  58. //                        Key_Scan(GPIOA,GPIO_Pin_7)*4+
  59. //                        Key_Scan(GPIOA,GPIO_Pin_14)*5+
  60. //                        Key_Scan(GPIOA,GPIO_Pin_15)*6+
  61. //                        Key_Scan(GPIOB,GPIO_Pin_10)*7+
  62. //                        Key_Scan(GPIOB,GPIO_Pin_11)*8;
  63. //        return key;
  64. //}
  65. void KeyScan(void)
  66. {        
  67.         if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_4) == 0) //K1
  68.         {        
  69.                 switch(Item)
  70.                 {
  71.                         case 2:R+=5.0;
  72.                                    if(R >= 35.0) R = 35.0;
  73. //                                   sprintf(buf,"DS16(6,60,'設置長度:%.1f ',10)\r\n",R);
  74. //                                   GpuSend(buf);        
  75.                                break;  //第2問按下S4增加距離

  77.                         case 3:angle+=10.0;
  78.                                    if(angle >= 180.0)
  79.                                              angle = 180.0;
  80. //                                   sprintf(buf,"DS16(6,80,'設置角度:%.1f ',10)\r\n",angle);
  81. //                                   GpuSend(buf);        
  82.                                break;  //第3問按下S4增加角度;  
  83.                         
  84.                         case 5:R+=5.0;
  85.                                    if(R >= 35.0) R = 35.0;
  86. //                                   sprintf(buf,"DS16(6,100,'設置半徑:%.1f ',10)\r\n",R);
  87. //                                   GpuSend(buf);
  88.                                    break;

  90.                         case 6:R+=5.0;
  91.                                    if(R >= 35.0) R = 35.0;
  92. //                                   sprintf(buf,"DS16(6,100,'設置半徑:%.1f ',10)\r\n",R);
  93. //                                   GpuSend(buf);
  94.                                    break;
  95.                                    

  97.                         default:break;
  98.                 }                                
  99.         }

  101.         if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_5) == 0) //K2
  102.         {
  103.                 switch(Item)
  104.                 {
  105.                         case 2:R-=5.0;
  106.                                    if(R <= 15.0) R = 15.0;
  107. //                                   sprintf(buf,"DS16(6,60,'設置長度:%.1f ',10)\r\n",R);
  108. //                                   GpuSend(buf);        
  109.                                break;  //第2問按下S4增加距離

  111.                         case 3:angle-=10.0;
  112.                                    if(angle <= 0.0)
  113.                                              angle = 0.0;
  114. //                                   sprintf(buf,"DS16(6,80,'設置角度:%.1f ',10)\r\n",angle);
  115. //                                   GpuSend(buf);        
  116.                                break;  //第3問按下S4增加角度;  
  117.                         
  118.                         case 5:R-=5.0;
  119.                                    if(R <= 15.0) R = 15.0;
  120. //                                   sprintf(buf,"DS16(6,100,'設置半徑:%.1f ',10)\r\n",R);
  121. //                                   GpuSend(buf);
  122.                                    break;

  124.                         case 6:R-=5.0;
  125.                                    if(R <= 15.0) R = 15.0;
  126. //                                   sprintf(buf,"DS16(6,100,'設置半徑:%.1f ',10)\r\n",R);
  127. //                                   GpuSend(buf);
  128.                                    break;
  129.                                    

  131.                         default:break;
  132.                 }               
  133.         }
  134.         
  135.         if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_6) == 0) //K3
  136.         {
  137.                 switch(Item)
  138.                 {
  139.                         case 1:Q1_Start = 1;
  140. //                                   sprintf(buf,"DS16(6,120,'開始!',10)\r\n");
  141. //                                   GpuSend(buf);
  142.                                    break;

  144.                         case 2:Q2_Start = 1;
  145.                                    //sprintf(buf,"DS16(6,120,'開始!',10)\r\n");
  146.                                    //GpuSend(buf);
  147.                                    break;

  149.                         case 3:Q3_Start = 1;
  150.                                   // sprintf(buf,"DS16(6,120,'開始!',10)\r\n");
  151.                                    //GpuSend(buf);
  152.                                    break;  

  154.                         case 4:Q4_Start = 1;
  155.                                   // sprintf(buf,"DS16(6,120,'開始!',10)\r\n");
  156.                                    //GpuSend(buf);
  157.                         break;
  158.                                    
  159. //                        case 5:Q5_Start = 1;
  160. //                                   RoundDir = !RoundDir;
  161. //                                   if(RoundDir == 1)
  162. //                                         // sprintf(buf,"DS16(6,120,'順時針旋轉!',10)\r\n");
  163. //                                   else
  164. //                                              sprintf(buf,"DS16(6,120,'逆時針旋轉!',10)\r\n");
  165. //                                   GpuSend(buf);break;
  166.                                    
  167.                         case 6:Q6_Start = 1;               
  168.                               // sprintf(buf,"DS16(6,120,'開始!',10)\r\n");
  169.                                   // GpuSend(buf);
  170.                                         break;   

  172.                         default:break;
  173.                 }        
  174.         }         

  176.         if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_7) == 0)  //K4
  177.         {
  178.                 Item++;
  179.                 if(Item>6)                //共有7問
  180.                         Item = 0;
  181.                 //sprintf(buf,"DS16(6,40,'第%d問',10)\r\n",Item);
  182.                 //GpuSend(buf);
  183.         }
  184. }


復制代碼



所有資料51hei提供下載:
風力擺控制系統+操作說明.rar (1.09 MB, 下載次數: 329)


評分

參與人數 2黑幣 +55 收起 理由
造夢Sir + 5 共享資料的黑幣獎勵！
admin + 50 共享資料的黑幣獎勵！

查看全部評分

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

使用道具 舉報

沙發
ID:1 發表于 2018-8-9 01:56 | 只看該作者
好資料，51黑有你更精彩!!!
回復

使用道具 舉報

板凳
ID:250245 發表于 2018-8-11 20:52 | 只看該作者
謝謝樓主
回復

使用道具 舉報

地板
ID:581864 發表于 2019-7-10 20:08 | 只看該作者
壓縮包解壓不了，顯示壓縮包損壞或文件格式未知。
這是怎么回事？
回復

使用道具 舉報

5#
ID:328014 發表于 2019-7-10 21:43 | 只看該作者
lxh666 發表于 2019-7-10 20:08
壓縮包解壓不了，顯示壓縮包損壞或文件格式未知。
這是怎么回事？

貌似我用winrar可以解壓
回復

使用道具 舉報

6#
ID:380985 發表于 2019-7-25 23:24 來自手機 | 只看該作者
好資料，正好學習
回復

使用道具 舉報

7#
ID:567506 發表于 2019-7-26 10:57 | 只看該作者
膜拜大佬
回復

使用道具 舉報

8#
ID:496604 發表于 2019-7-28 22:39 | 只看該作者
膜拜大佬
回復

使用道具 舉報

9#
ID:591400 發表于 2019-7-31 14:57 | 只看該作者
MPU6050有姿態融合嗎？數據漂移情況如何？
回復

使用道具 舉報

10#
ID:484435 發表于 2019-8-1 09:25 | 只看該作者
謝謝樓主分享
回復

使用道具 舉報

11#
ID:484435 發表于 2019-8-1 09:37 | 只看該作者
謝謝樓主分享
回復

使用道具 舉報

12#
ID:586030 發表于 2019-8-2 11:07 | 只看該作者
資料是好資料，但是希望你能夠標明你是轉載的，我正好剛看過原作者的帖子
回復

使用道具 舉報

13#
ID:444495 發表于 2019-8-3 17:14 | 只看該作者
感謝分享
回復

使用道具 舉報

14#
ID:594722 發表于 2019-8-3 18:03 | 只看該作者
厲害了樓主
回復

使用道具 舉報

15#
ID:719608 發表于 2020-9-20 17:54 | 只看該作者
加油，不錯！
回復

使用道具 舉報

16#
ID:871474 發表于 2021-3-14 14:58 | 只看該作者
為什么壓縮包里面的程序沒有看到PID部分哇
回復

使用道具 舉報

17#
ID:328014 發表于 2021-3-14 15:21 | 只看該作者
可樂我要檸檬味 發表于 2021-3-14 14:58
為什么壓縮包里面的程序沒有看到PID部分哇

是這個嗎Subject.c
  1. #include "Subject.h"
  2. #include "Data.h"
  3. #include "math.h"
  4. #include "moto_PWM.h"
  5. #include "math.h"
  6. #define H    79.0                  //萬向節距離地面的高度
  7. float R=25.0;           //擺動弧度的大小半徑
  8. #define T    1410.0   //擺動的周期
  9. float angle = 45.0;

  11. extern Data  PID_data;
  12. extern Data  PID_data1;
  13. uint8_t RoundDir=1;
  14. //const float Phase;
  15. void Subject_1(void)
  16. {
  17.         float A=0.0;
  18.         static uint32_t MoveTimeCnt = 0;
  19.         float set_y = 0.0;
  20.         float Normalization = 0.0;
  21.         float Omega = 0.0;
  22.         MoveTimeCnt+=5;
  23.         Normalization=(float)MoveTimeCnt/T;
  24.         Omega=2*3.1415926*Normalization;      //求一個角度 隨時間變化的 每5毫秒加2*PI*5/T度   來求相位
  25.        
  26.         A=atan((R/79.0f))*57.2958f;                                                //最大角度 擺的最大高度
  27.        
  28.         set_y=A*sin(Omega);                        //軌跡函數
  29.        
  30.         PID_data.setPoint=0;
  31.         PID_data.P=170;
  32.         PID_data.I=5.8;
  33.         PID_data.D=500;
  34.        
  35.        
  36.         PID_data1.setPoint=set_y;
  37.         PID_data1.P=150;
  38.         PID_data1.I=6.8;
  39.         PID_data1.D=500;
  40.        
  41.         PID_data.PWM=PID_Calc1();
  42.         PID_data1.PWM=PID_Calc2();
  43.        
  44.         if(PID_data.PWM>1500)
  45.                 PID_data.PWM=1500;
  46.         /*else if(PID_data.PWM>0&&PID_data.PWM<450)
  47.                 PID_data.PWM=450;*/
  48.         if(PID_data.PWM<-1500)
  49.                 PID_data.PWM=-1500;
  50.                 /*else if(PID_data.PWM<0&&PID_data.PWM>-450)
  51.                 PID_data.PWM=-450;*/
  52.                
  53.         if(PID_data1.PWM>1500)
  54.                 PID_data1.PWM=1500;
  55.         if(PID_data1.PWM<-1500)
  56.                 PID_data1.PWM=-1500;
  57.        
  58.         moto_PWM(PID_data.PWM,PID_data1.PWM);
  59.        
  60. }



  64. void Subject_2(void)
  65. {
  66.         float A=0.0;
  67.         static uint32_t MoveTimeCnt = 0;
  68.         float set_x = 0.0;
  69.         float Normalization = 0.0;
  70.         float Omega = 0.0;
  71.         MoveTimeCnt+=5;
  72.         Normalization=(float)MoveTimeCnt/T;
  73.         Omega=2*3.1415926*Normalization;
  74.         A=atan((R/79.0f))*57.2958f;
  75.         set_x=A*sin(Omega);
  76.        
  77.         PID_data.setPoint=set_x;
  78.         PID_data.P=191;
  79.         PID_data.I=5.9;
  80.         PID_data.D=420;
  81.        
  82.        
  83.         PID_data1.setPoint=0;
  84.         PID_data1.P=100;
  85.         PID_data1.I=5.8;
  86.         PID_data1.D=500;
  87.        
  88.         PID_data.PWM=PID_Calc1();
  89.         PID_data1.PWM=PID_Calc2();
  90.        
  91.         if(PID_data.PWM>1500)
  92.                 PID_data.PWM=1500;
  93.         if(PID_data.PWM<-1500)
  94.                 PID_data.PWM=-1500;
  95.        
  96.         if(PID_data1.PWM>1500)
  97.                 PID_data1.PWM=1500;
  98.         if(PID_data1.PWM<-1500)
  99.                 PID_data1.PWM=-1500;
  100.        
  101.         moto_PWM(PID_data.PWM,PID_data1.PWM);
  102.        
  103. }


  106. void Subject_3(void)
  107. {
  108.         //const float priod = 1410.0;  //單擺周期(毫秒)
  109.                      //相位補償 0, 10   20   30   40   50   60   70   80   90   100  110  120  130  140  150  160  170 180
  110.         const float Phase[19]= {0,-0.1,-0.05,0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.05,0.05,0.05,0.07,0};
  111.         static uint32_t MoveTimeCnt = 0;
  112.         float set_x = 0.0;
  113.         float set_y = 0.0;
  114.         float Ax = 0.0;
  115.         float Ay = 0.0;
  116.         float A = 0.0;
  117.         uint32_t pOffset = 0;
  118.         float Normalization = 0.0;
  119.         float Omega = 0.0;
  120.        
  121.         pOffset = (uint32_t)(angle/10.0f);                         //相位補償數組下標
  122.         MoveTimeCnt += 5;                                                         //每5ms運算1次
  123.         Normalization = (float)MoveTimeCnt/T;         //對單擺周期歸一化
  124.         Omega = 2.0*3.14159*Normalization;                         //對2π進行歸一化處理
  125.         A = atan((R/79.0f))*57.2958f;//根據擺幅求出角度A,88為擺桿離地高度                                                                  
  126.         Ax = A*cos(angle*0.017453);         //計算出X方向擺幅分量0.017453為弧度轉換
  127.         Ay = A*sin(angle*0.017453);         //計算出Y方向擺幅分量
  128.         set_x = Ax*sin(Omega);                  //計算出X方向當前擺角
  129.         set_y = Ay*sin(Omega+Phase[pOffset]); //計算出Y方向當前擺角
  130.                
  131.         PID_data.setPoint=set_y;
  132.         PID_data.P=180;
  133.         PID_data.I=6.5;
  134.         PID_data.D=500;
  135.        
  136.        
  137.         PID_data1.setPoint=set_x;
  138.         PID_data1.P=110;
  139.         PID_data1.I=5.9;
  140.         PID_data1.D=500;         
  141.        
  142.         PID_data.PWM=PID_Calc1();
  143.         PID_data1.PWM=PID_Calc2();
  144.        
  145.         if(PID_data.PWM>1700)
  146.                 PID_data.PWM=1700;

  148.         if(PID_data.PWM<-1700)
  149.                 PID_data.PWM=-1700;

  151.        
  152.         if(PID_data1.PWM>1700)
  153.                 PID_data1.PWM=1700;

  155.         if(PID_data1.PWM<-1700)
  156.                 PID_data1.PWM=-1700;

  158.         moto_PWM(PID_data.PWM,PID_data1.PWM);
  159. }
復制代碼
回復

使用道具 舉報

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

本版積分規則

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

Powered by 單片機教程網

快速回復 返回頂部 返回列表
主站蜘蛛池模板： 欧美精品区 | 中文字幕一区二区三区精彩视频 | 国产精品区二区三区日本 | 久久精品欧美一区二区三区麻豆 | 99国产精品久久久 | 人人干在线 | 成人欧美一区二区三区黑人孕妇 | 久久成人高清视频 | 国产精品黄视频 | 男人天堂av网站 | 亚洲va中文字幕 | 午夜影院黄 | 人人人人干| 日韩国产中文字幕 | 最新国产精品精品视频 | 国产精品高清在线 | 久久高清亚洲 | 亚洲精品9999| 亚洲一区二区久久 | www日本在线 | 欧美在线一区二区三区 | 欧美国产精品 | 国产成人福利在线 | av黄色网 | 国产欧美日韩一区二区三区 | 97色在线观看免费视频 | 第四色狠狠 | 色婷婷激情综合 | 国产精品亚洲一区 | 久久一日本道色综合久久 | 色橹橹欧美在线观看视频高清 | 精品久久久久久久久久久 | 日韩一区二区免费视频 | 四虎影院免费在线播放 | 国产成人黄色 | 欧美精品a∨在线观看不卡 欧美日韩中文字幕在线播放 | 亚洲欧美日韩国产综合 | 日韩www视频 | 98久久 | 日韩在线一区二区 | 亚洲一区二区在线视频 |