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

QQ登錄

只需一步，快速開始

搜索
查看: 2998|回復(fù): 0
打印 上一主題 下一主題
收起左側(cè)

STM32自平衡方塊程序，分享資料

[復(fù)制鏈接]
跳轉(zhuǎn)到指定樓層
樓主
ID:505762 發(fā)表于 2020-4-16 22:42 | 只看該作者 回帖獎(jiǎng)勵(lì) |倒序?yàn)g覽 |閱讀模式
自平衡方塊，拿去自學(xué)
  1. /*       
  2. 知乎：天下行走                                                       
  3. CSDN：技術(shù)創(chuàng)造無(wú)限可能                               
  4. 閑魚：抉擇之刃銀
  5. */

  7. /*
  8. 針對(duì)角度環(huán)的期望角度為速度環(huán)輸出+機(jī)械中值的提問：
  9. 【一】
  10. 問：
  11.         當(dāng)我在小車（平衡小車）前側(cè)綁一塊小鐵塊，這會(huì)使得機(jī)械中值后傾，=-27度，然后我將車子扶在機(jī)械中值的角度位置然后開機(jī)，但車子一開機(jī)就往前跑。為什么？
  12. 答：
  13.         問題分析：
  14.                                         起始速度偏差=0（因?yàn)闆]有動(dòng)輪子呀）——>外環(huán)輸出=0（即內(nèi)環(huán)期望角度=0）
  15.                                         But：開局實(shí)際角度= -27度——>開局內(nèi)環(huán)輸出就等于(-27-0)*(-1) = +27  //Kp為負(fù)數(shù)，這里設(shè)為-1
  16.                                         即根本是因?yàn)殚_局時(shí) 外環(huán)輸出！=機(jī)械中值(這里機(jī)械中值可以等效說成實(shí)際角度，但不是任何時(shí)候都可以等效說成的。譬如我開機(jī)時(shí)小車不擺在機(jī)械中值位置時(shí)就不能等效說成。)
  17.                                         =====>>>>所以開局時(shí)車子肯定往前跑�。�！                       
  18.         解決思想：
  19.                                         起始Target=Real就可以了。
  20.         解決方法：
  21.                                         測(cè)機(jī)械中值譬如 = -27度，則使得內(nèi)環(huán)期望角度=外環(huán)輸出 + 機(jī)械中值。       
  22.                                        
  23. 【二】
  24. 若：外環(huán)輸出==機(jī)械中值，即開局速度偏差=0時(shí)，機(jī)械中值=0則外環(huán)輸出=0，即內(nèi)環(huán)輸入=0。
  25. 又有內(nèi)環(huán)反饋=0，所以這時(shí)候開局內(nèi)環(huán)輸出就等于(0-0)*(-1) = 0===>車子必然直立靜止在原地。

  27. 【三】
  28. 若：外環(huán)輸出！=機(jī)械中值，即開局速度偏差=0時(shí)，機(jī)械中值!=0，但我開局不把車子擺在機(jī)械中值位置。譬如舉例當(dāng)機(jī)械中值=-27度，我開機(jī)將車子實(shí)際角度擺在0度位置。則此時(shí)
  29. 速度偏差=0則外環(huán)輸出=0，即內(nèi)環(huán)輸入=0。
  30. 又有內(nèi)環(huán)反饋=0===>理論上小車直立靜止，BUT！車子根本不在機(jī)械中值位置，又談何直立靜止，車子會(huì)根據(jù)重心向前倒下，直立環(huán)立刻發(fā)揮作用-->
  31. -->若小鐵快非常非常輕，還有可能救回來；若鐵塊不輕，則必然車子一直不斷向前跑不會(huì)停下；若小鐵塊過重，系統(tǒng)來不及響應(yīng)，車子會(huì)直接向前倒下；
  32. 這，就是結(jié)果。
  33. */       


  36. /*
  37. 【硬件占用的GPIO的框架】
  38. 編碼器        ||AB相—PA0/PA1
  39. 電機(jī)                ||PWM——PA8/PA11、PA6/PA7
  40. MPU6050 ||INT——PB5
  41. MPU6050 ||IIC——SCL--PB4/SDA--PB3
  42. OLED                ||IIC——SCL--PB8/SDA--PB9
  43. */
  44. #include "stm32f10x.h"
  45. #include "sys.h"

  47. float Pitch,Roll,Yaw;                                                                //角度
  48. short gyrox,gyroy,gyroz;                                                //陀螺儀--角速度
  49. short aacx,aacy,aacz;                                                                //加速度
  50. int Encoder_Momentum;                                                                //編碼器數(shù)據(jù)（速度）

  52. int PWM_MAX=7000,PWM_MIN=-7000;                        //PWM限幅變量
  53. int MOTO1,MOTO2;                                                                                //電機(jī)裝載變量
  54.                
  55. extern int Velocity_Momentum_out;                //輸出變量

  57. int main(void)       
  58. {
  59.         delay_init();
  60.         NVIC_Config();
  61.         uart1_init(115200);       
  62.        
  63.         OLED_Init();
  64.         OLED_Clear();
  65.        
  66.         MPU_Init();
  67.         mpu_dmp_init();
  68.         MPU6050_EXTI_Init();
  69.        
  70.         Encoder_TIM2_Init();
  71.         Encoder_TIM4_Init();
  72.         PWM_Init_TIM1(0,7199);        //CH1——PA8、CH4——PA11
  73.         PWM_Init_TIM3(0,7199);        //CH1——PA6、CH2——PA7       
  74.         OLED_Clear();
  75.         OLED_ShowString(5,0,"P",12);
  76.         OLED_ShowChar(5,2,'M',12);
  77.         OLED_ShowString(64,2,"Stop:",12);       
  78.   while(1)
  79.         {
  80.                 OLED_Float(0,15,Pitch,1);                                                                        //角度                                               
  81.                 OLED_Float(0,70,Velocity_Momentum_out,1);        //外環(huán)輸出
  82.                 OLED_Float(2,5+15,Encoder_Momentum,1);                //速度讀取
  83.                 OLED_Float(3,5,MOTO1,1 );                                                                        //電機(jī)PWM
  84.                
  85.                 OLED_ShowNumber(64+40,2,stop,1,12);                                //停止位
  86.         }        
  87. }
復(fù)制代碼


自平衡方塊GFP.7z

258.13 KB, 下載次數(shù): 38, 下載積分: 黑幣 -5

評(píng)分

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

查看全部評(píng)分

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

使用道具 舉報(bào)

本版積分規(guī)則

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

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

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