上一篇文章：看完這篇文章，還不會做平衡小車，你來打我。

/*******************************************************************函數(shù)功能：直立PD控制入口參數(shù)：角度、機械平衡角度（機械中值）、角速度返回 值：直立控制PWM作    者：公眾號【大魚機器人】******************************************************************/int balance_UP(float Angle,float Mechanical_balance,float Gyro){  float Bias;//角度誤差   int balance;//直立環(huán)計算出來的電機控制pwm Bias=Angle-Mechanical_balance;  //===求出平衡的角度中值和機械相關 balance=balance_UP_KP*Bias+balance_UP_KD*Gyro;  //===計算平衡控制的電機PWM PD控制 kp是P系數(shù) kd是D系數(shù)  return balance;}

2.1 直立環(huán) P 范圍確定：

balance=balance_UP_KP*Bias+balance_UP_KD*Gyro;

2.2 直立環(huán) P 極性確定：

極性也就是符號，P到底是給正的，也是負的。

直接給kp正負值，然后觀察現(xiàn)象： 

正常出現(xiàn)的現(xiàn)象是負反饋，小車往那邊倒，電機轉動使得小車往要倒的方向去追。使得小車能夠往反方向站起來！

如果出現(xiàn)正反饋，車往哪邊倒，電機轉動使得小車快速倒下。這種現(xiàn)象就是不對的。

2.3 直立環(huán) P 大小確定：

慢慢試錯，從小到大，響應慢慢加快也就是小車倒下后恢復直立的時間越來越短，直到小車出現(xiàn)大幅度的低頻抖動！

此時的P可以確定。

2.4 直立環(huán) D 極性確定：

D的極性較為好確定，設P為0，D給正負值，分別去試，看效果。

當拿起小車進行旋轉時，小車的輪子應該是小車旋轉方向相同，此時說明極性是對的。

如果小車的輪子轉動和小車的轉動方向不相同，說明此時極性是反的！

2.5 直立環(huán) D 大小確定：

/**************************************入口參數(shù)：電機編碼器的值返回 值：速度控制PWM作 者：公眾號【大魚機器人】**************************************/int velocity(int encoder_left,int encoder_right){     static float Velocity,Encoder_Least,Encoder,Movement; static float Encoder_Integral; //=============速度PI控制器=======================//  Encoder_Least =(Encoder_Left+Encoder_Right)-0;  //===獲取最新速度偏差==測量速度（左右編碼器之和）-目標速度（此處為零）     Encoder *= 0.7;          //===一階低通濾波器        Encoder += Encoder_Least*0.3; //===一階低通濾波器     Encoder_Integral +=Encoder; //===積分出位移 積分時間：10ms if(Encoder_Integral>10000) Encoder_Integral=10000;  //===積分限幅 if(Encoder_Integral<-10000) Encoder_Integral=-10000;     //===積分限幅   Velocity=Encoder*velocity_KP+Encoder_Integral*velocity_KI;     //===速度控制      if(pitch<-40||pitch>40)   Encoder_Integral=0;       //===電機關閉后清除積分 return Velocity;}

3.1 速度環(huán) P 范圍確定：

同樣的，和直立環(huán)P的大小范圍確定一樣，我們需要得到電機編碼器的最大值和PWM的最大值的關系！

從程序中可以看到，我們應該比較的是，2個電機的速度偏差和pwm的關系。

比如：用STM32定時器的正交解碼模式對電機進行測速，10ms一次。

小車電機滿速旋轉時，左右兩個電機，編碼器相加可達160。

假設速度偏差（實際測量值與理想值）達到50%時滿轉。

那么有，160/2=80，7200/80=90，也就說kp最大為90。

(注意，這里只是在假設50%的前提下).

90只是一個參考值，具體多少，還是需要根據(jù)，實際測試的效果。

3.2 速度環(huán) P 極性確定：

確定P的極性，需要關閉前文的直立環(huán)，也就是說整個系統(tǒng)的控制參數(shù)只能有速度環(huán)的P。

單單靠直立環(huán)控制小車，小車能短暫直立，但會出現(xiàn)往前走或往后走，然后倒下，那么速度環(huán)就是用來抑制此現(xiàn)象的出現(xiàn)。

從上文程序中可以看到：

 Encoder_Least =(Encoder_Left+Encoder_Right)-0;       //===獲取最新速度偏差==測量速度（左右編碼器之和）-目標速度（此處為零）

這句程序的意思就是，獲取最新速度偏差，控制小車目標速度為0。

直立環(huán)中控制小車不倒下是用來控制小車的角度，所以直立環(huán)的機械中值是：角度

速度環(huán)控制小車不倒下是用來控制小車的速度，所以速度環(huán)的“中值”就是：速度為0

應該不難理解！

那么如何抑制小車速度為0呢?

既然我們可以知道小車的當前速度，只要速度環(huán)的P為正反饋即可，意思就是假如向前倒，那么小車就要以更快的速度向前沖，保持直立。

同樣的，屏蔽前文的直立環(huán)，分別給速度環(huán)P正負值，看現(xiàn)象。

正反饋的現(xiàn)象為：

當旋轉其中一個輪子，兩個輪子往相同方向旋轉，到速度最大值。此時應該為正反饋。此時的現(xiàn)象說明，速度環(huán)的P極性是對的！

如果出現(xiàn)旋轉其中一個輪子，另外一個輪子往反方向轉動，讓偏差趨向于零。這就是負反饋，此時說明P極性錯誤！

3.3 速度環(huán) P 大小確定：

確定P極性和大小之后，由于P和I有比例關系且P為I的200倍！P和I的大小可以一同調(diào)試，可以將P和I慢慢從小到大的參數(shù)去試，觀看小車效果。

如果出現(xiàn)以下效果：

1、小車放在地上，慢慢的，隨著時間越來越長，小車會來回晃蕩，此時可以認為P和I的參數(shù)過小。

2、小車放在地上，用手去推，如果小車無法回到初始位置，一直來回晃蕩，來回晃蕩的時候，車身出現(xiàn)較為大的傾斜，此時可以認為P和I的參數(shù)過大。如果車身沒有出現(xiàn)較大的傾斜，只是小車來回晃蕩，此時可以認為P和I的參數(shù)過小。

關于速度環(huán)的初步調(diào)試，大致就講到這里，這種試錯的方法是較為愚鈍的，但卻是較為方便且簡單的一種方法。