上一篇文章：看完這篇文章，還不會(huì)做平衡小車，你來(lái)打我。

/*******************************************************************函數(shù)功能：直立PD控制入口參數(shù)：角度、機(jī)械平衡角度（機(jī)械中值）、角速度返回 值：直立控制PWM作    者：公眾號(hào)【大魚機(jī)器人】******************************************************************/int balance_UP(float Angle,float Mechanical_balance,float Gyro){  float Bias;//角度誤差   int balance;//直立環(huán)計(jì)算出來(lái)的電機(jī)控制pwm Bias=Angle-Mechanical_balance;  //===求出平衡的角度中值和機(jī)械相關(guān) balance=balance_UP_KP*Bias+balance_UP_KD*Gyro;  //===計(jì)算平衡控制的電機(jī)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 極性確定：

極性也就是符號(hào)，P到底是給正的，也是負(fù)的。

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

正常出現(xiàn)的現(xiàn)象是負(fù)反饋，小車往那邊倒，電機(jī)轉(zhuǎn)動(dòng)使得小車往要倒的方向去追。使得小車能夠往反方向站起來(lái)！

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

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

慢慢試錯(cuò)，從小到大，響應(yīng)慢慢加快也就是小車倒下后恢復(fù)直立的時(shí)間越來(lái)越短，直到小車出現(xiàn)大幅度的低頻抖動(dòng)！

此時(shí)的P可以確定。

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

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

當(dāng)拿起小車進(jìn)行旋轉(zhuǎn)時(shí)，小車的輪子應(yīng)該是小車旋轉(zhuǎn)方向相同，此時(shí)說(shuō)明極性是對(duì)的。

如果小車的輪子轉(zhuǎn)動(dòng)和小車的轉(zhuǎn)動(dòng)方向不相同，說(shuō)明此時(shí)極性是反的！

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

/**************************************入口參數(shù)：電機(jī)編碼器的值返回 值：速度控制PWM作 者：公眾號(hào)【大魚機(jī)器人】**************************************/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;  //===獲取最新速度偏差==測(cè)量速度（左右編碼器之和）-目標(biāo)速度（此處為零）     Encoder *= 0.7;          //===一階低通濾波器        Encoder += Encoder_Least*0.3; //===一階低通濾波器     Encoder_Integral +=Encoder; //===積分出位移 積分時(shí)間：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;       //===電機(jī)關(guān)閉后清除積分 return Velocity;}

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

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

從程序中可以看到，我們應(yīng)該比較的是，2個(gè)電機(jī)的速度偏差和pwm的關(guān)系。

比如：用STM32定時(shí)器的正交解碼模式對(duì)電機(jī)進(jìn)行測(cè)速，10ms一次。

小車電機(jī)滿速旋轉(zhuǎn)時(shí)，左右兩個(gè)電機(jī)，編碼器相加可達(dá)160。

假設(shè)速度偏差（實(shí)際測(cè)量值與理想值）達(dá)到50%時(shí)滿轉(zhuǎn)。

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

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

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

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

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

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

從上文程序中可以看到：

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

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

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

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

應(yīng)該不難理解！

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

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

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

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

當(dāng)旋轉(zhuǎn)其中一個(gè)輪子，兩個(gè)輪子往相同方向旋轉(zhuǎn)，到速度最大值。此時(shí)應(yīng)該為正反饋。此時(shí)的現(xiàn)象說(shuō)明，速度環(huán)的P極性是對(duì)的！

如果出現(xiàn)旋轉(zhuǎn)其中一個(gè)輪子，另外一個(gè)輪子往反方向轉(zhuǎn)動(dòng)，讓偏差趨向于零。這就是負(fù)反饋，此時(shí)說(shuō)明P極性錯(cuò)誤！

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

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

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

1、小車放在地上，慢慢的，隨著時(shí)間越來(lái)越長(zhǎng)，小車會(huì)來(lái)回晃蕩，此時(shí)可以認(rèn)為P和I的參數(shù)過(guò)小。

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

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