標(biāo)題: STM32自平衡方塊程序，分享資料 [打印本頁(yè)]
作者: stc89c52wzy    時(shí)間: 2020-4-16 22:42
標(biāo)題: STM32自平衡方塊程序，分享資料
自平衡方塊，拿去自學(xué)
  1. /*       
  2. 知乎：天下行走                                                       
  3. CSDN：技術(shù)創(chuàng)造無(wú)限可能                               
  4. 閑魚(yú)：抉擇之刃銀
  5. */

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

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





歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1