第1步：理論

自平衡機器人的問題是倒立擺的問題。為了抵消機器人向前或向后下落的力，我們需要一種機制，使其重心直接保持在其樞轉(zhuǎn)點的上方。這個樞軸點將是我們的輪軸。我們的反作用策略將通過驅(qū)動機器人的車輪沿其下降的方向進行。

然而，問題是停在那里。如果我們有一個簡單的反饋回路來檢查機器人正朝著哪個方向下降并沿著那個方向驅(qū)動車輪，那么我們的機器人將會固有地振蕩并崩潰。

因此我們的戰(zhàn)略將涉及實施PID控制器來驅(qū)動車輪以受控的數(shù)學(xué)方式來回，響應(yīng)機器人下落的方向，它下降的速度，到目前為止傾斜的量，以及所有這三個變量之間的關(guān)系。

有關(guān)如何實現(xiàn)的細節(jié)將在本教程的PID部分進一步說明。

步驟2：構(gòu)建軸和控制中心的Chasis

機箱：

將膠合板切成3塊，每塊寬9厘米，長14.5厘米，這些將成為機器人底盤的三個平臺。

在每個平臺的角落鉆6mm孔，距離邊緣10mm（見圖）。

標(biāo)記三個平臺：頂部，中部和底部。

在頂級平臺上鉆出Arduino Uno和L298N驅(qū)動板的孔。

測量中間平臺的中心點，并標(biāo)出小型面包板的區(qū)域以及用于連接器的位置（參見圖表）。

安裝在底部平臺上的電機的鉆孔，以及電機上的電線孔（見圖）。

將M6螺紋桿切成四個25厘米的小塊。

將每個部件滑入平臺的相應(yīng)孔中，用M6墊圈和M6螺母固定每個孔的兩側(cè)。

您的平臺應(yīng)相距約9厘米，剩余長度應(yīng)從頂部平臺伸出。

測量每個平臺的傾斜角度，并調(diào)整每個螺母，使其全部與地面齊平。

縱向切割其中一個廚房海綿，并將每個橡膠到頂部平臺的兩半作為保險杠。此步驟僅用于測試。

軸：

在您的中心鉆一個6毫米的洞車輪。

將30毫米M6螺紋桿滑入孔中。用M6墊圈和M6螺母固定外端，并用另一個M6螺母固定內(nèi)端。

將聯(lián)軸器安裝到M6螺紋桿的內(nèi)端，并擰緊固定螺釘將其固定到位。

將耦合器的開口端連接到電機軸上，并擰緊固定螺釘將其固定到位。確保兩個輪子與電機本身的距離相同。

從卷筒上切下兩根40厘米長的電源線，然后將它們焊接到電機的端子上。

拿起5毫米木螺釘，按照底盤構(gòu)造階段制作的導(dǎo)孔，將電機連接到底部平臺。

將電機穿過底部平臺的中心孔。

控制中心：

下載本節(jié)頂部的dxf文件（bug lounge cut file.dxf）。

激光切割出2mm有機玻璃的文件。

根據(jù)本節(jié)頂部的圖表組裝零件。

將廚房海綿放在底部平臺上（如果需要，可以用膠水或雙層膠帶）。

將控制中心放在海綿上。

擠壓海綿，將兩塊小木塊（約15x15毫米）放在控制中心和中間平臺之間。它是一種易于拆卸的方式來放入和放出我們的控制箱。我們在上面的圖片上實現(xiàn)。

第3步：構(gòu)建電路

電機驅(qū)動器

L298N的使能引腳用于控制使用PWM（脈沖寬度調(diào)制）的電機速度，而驅(qū)動器的In1-4引腳用于切換電機的方向。以下是描述本節(jié)頂部Fritzing圖的說明。

將L298N的EnA引腳連接到Arduino的數(shù)字引腳6.

將In1引腳連接到數(shù)字引腳5，將In2引腳連接到數(shù)字引腳3.

將L298N的EnB引腳連接到Arduino的數(shù)字引腳11.

將In3引腳連接到數(shù)字引腳13，將In4引腳連接到數(shù)字引腳12.

取下5V_EN跳線，以便為Arduino提供驅(qū)動器的電源。

將5V螺絲端子從L298N連接到Arduino的Vin引腳。

將其中一個電機的正負電源線連接到MotorA螺絲端子。

將其他電機的正負電源線連接到MotorB螺絲端子。

切掉另一根電源線，將紅線連接到L298N的VMS引腳，黑線連接到L298N的GND引腳。紅線的另一端應(yīng)連接到Wago連接器。

將螺絲端子放在迷你面包板的末端。

切掉另一根電源線并將其連接到母筒插孔。然后紅色端應(yīng)連接到Wago連接器，以完成電路一直到L298N的VMS引腳，而黑色端將進入我們之前放入迷你面包板的螺絲端子。

將Arduino的GND引腳連接到與迷你面包板中的母筒插孔相同的線路中。這將確保我們的系統(tǒng)基礎(chǔ)全部連接。

在迷你面包板上放置另一個螺絲端子，將我們之前放入L298N的GND引腳的另一端連接到此端子。確保它也連接到我們在上一步中建立的地線。我們的接地電路現(xiàn)在應(yīng)該完整了。 （如果這部分令人困惑，請查看圖像。）

BNO055絕對定向傳感器

BNO055是一款9自由度傳感器。它將來自加速度計，陀螺儀和磁力計的數(shù)據(jù)融合到絕對3D方向。 BNO055使用I2C通信，因此我們將它連接到Arduino Uno的A5和A4引腳。這將根據(jù)您選擇使用的Arduino的類型而改變。

將標(biāo)題條焊接到IMU的分線板中。

將IMU放在迷你面包板上。

使用跨接電纜將Arduino的5V引腳連接到迷你面包板。

將IMU的Vin引腳與來自迷你面包板上的Arduino的5V電纜串聯(lián)。

將IMU的GND引腳與來自迷你面包板上的Arduino的GND引腳串聯(lián)。

用更長的跨接電纜將其從IMU的SCL引腳連接到Arduino的A5引腳（它兼作SCL引腳）。

再用一根長跨接電纜將其從IMU的SDA引腳連接到Arduino的A4引腳（兼作SDA引腳）。

HC-SR04超聲波傳感器

HC-SR04傳感器是超聲波測距模塊，提供2cm至400cm的測量功能，精度為3mm。它的工作原理是發(fā)送脈沖，并檢測接收脈沖所需的時間。通過該脈沖測量的距離可以分解為一個簡單的等式：距離=（高水平時間*聲速）/2

將HC-SR04的VCC引腳與來自迷你面包板上的Arduino的5V電纜串聯(lián)。

將HC-SR04的GND引腳與來自迷你面包板上Arduino的GND電纜串聯(lián)。

將HC-SR04的Trig引腳連接到Arduino的Digital 4引腳。

將HC-SR04的Echo引腳連接到Arduino的Digital 2引腳。

使用第二個HC-SR04重復(fù)步驟1到4，但這次使用數(shù)字引腳7作為Trig，數(shù)字引腳8作為Echo。

電源

O 你的電機需要12V和每個約2安培，所以我們將使用外部電源來提供這種電力。 arduino本身將由電機驅(qū)動器的5V輸出供電。

切出5米來自電源線軸的長鏈。

剝?nèi)蓚?cè)的兩端。將電源螺絲端子的另一端連接到另一端。

裝配

將電子裝配到機箱很簡單。只需按照您在機箱構(gòu)造步驟中制作的導(dǎo)孔即可。

使用5mm木螺釘并使用塑料上的安裝孔將Arduino連接到頂部平臺案件。

取7mm墊片，將它們放在L298N電機驅(qū)動器下方，然后將M4螺栓穿過安裝孔并穿過墊片。

迷你面包板下面應(yīng)該有一塊雙面膠帶。取下此貼片的覆蓋物，將迷你面包板粘在中間平臺的中央。確保IMU位于平臺的中心，您可能需要調(diào)整面包板才能這樣做。

取另一塊雙面膠帶，將Wago連接器連接到中間平臺的邊緣。

使用扎帶將陰筒固定在其中一根螺桿上。

出于測試目的，將清潔海綿切成兩半并將每一半連接到頂部平臺的兩側(cè)，使用橡皮筋將其固定到位。您可以在機器人獨立后立即將其移除，但在此之前，這將使我們的電子設(shè)備免受損壞。

第4步：編碼：設(shè)置怪物類

為了以一種易于被其他開發(fā)者構(gòu)建的方式編程我們的Monster，我們將它作為一個類實現(xiàn)/圖書館。一個類由頭文件（.h）和源文件（.cpp）組成。頭文件定義了類中的所有內(nèi)容，而源文件包含實際的代碼實現(xiàn)。

我們將從頭文件開始：

#ifndef Monstro_h

#define Monstro_h

#include “Arduino.h”

class Monstro {

public：

Monstro（int leftForward， int leftBackward， int leftSpeedPin，

int rightForward， int rightBackward， int rightSpeedPin，

int trigA， int echoA， int trigB， int echoB）;

// Behavior

bool Update（）;

void Initialize（）;

private：

};

#endif

我們在這里所做的就是設(shè)置帶有構(gòu)造函數(shù)的頭文件，該構(gòu)造函數(shù)接收我們將用于與我們的傳感器和驅(qū)動程序交互的引腳。稍后我們將在介紹每個組成部分時添加此內(nèi)容。

#include Arduino.h語句只是確保我們可以訪問Arduino語言提供的常量和類型。

我們將使用Update（）函數(shù)在主循環(huán)期間調(diào)用某些行為，并且Initialize（）函數(shù)確保我們的傳感器和電機準(zhǔn)備就緒。在后面的步驟中有更多相關(guān)內(nèi)容。

我們的源文件將反映此頭文件：

#include “Arduino.h”

#include “monstro.h”

Monstro：：Monstro（int leftForward， int leftBackward， int leftSpeedPin，

int rightForward， int rightBackward， int rightSpeedPin，

int trigA， int echoA， int trigB， int echoB）

{

}

// Behavior

void Monstro：：Initialize（） {

}

bool Monstro：：Update（） {

}

再次，我們在這里所做的只是設(shè)置裸源文件的骨骼，同時確保我們在這里也包含Arduino.h引用，以及對頭文件的引用，以便我們也可以訪問它的定義。

步驟5：測量傾角（IMU）

由于Adafruit程序員編寫的庫，實現(xiàn)BNO055的代碼非常簡單。我們將使用Adafruit_BNO055驅(qū)動程序庫以及Adafruit統(tǒng)一傳感器庫。

讓我們首先更新我們的頭文件以與IMU交互。

#ifndef Monstro_h

#define Monstro_h

#include “Arduino.h”

#include

#include

#include

class Monstro {

public：

Monstro（int leftForward， int leftBackward， int leftSpeedPin，

int rightForward， int rightBackward， int rightSpeedPin，

int trigA， int echoA， int trigB， int echoB）;

// Behavior

bool Update（）;

void Initialize（）;

// IMU

volatile double xTilt;

volatile double yTilt;

volatile double zTilt;

private：

// IMU

Adafruit_BNO055 _bno;

void initializeIMU（）;

void readIMU（）;

};

#endif

我們在頭文件中添加了一些內(nèi)容。

首先，你會注意到三個include語句，它們確保我們可以訪問Adafruit庫以及imumaths.h庫，它們在實現(xiàn)IMU讀取時將需要它們的功能。

我們還添加了公共變量xTilt，yTilt和zTilt。這些是我們將在每個更新周期中存儲從IMU檢索的數(shù)據(jù)的地方。請注意，我們已將它們標(biāo)記為volatile，這是因為我們將在本教程后面的計時器中斷中使用它們。

我們還添加了一個BNO055對象（_bno），一個初始化函數(shù)來設(shè)置它，以及一個在更新周期中使用的讀取函數(shù)。

現(xiàn)在讓我們在源文件中實現(xiàn)這些功能：

#include “Arduino.h”

#include “monstro.h”

Monstro：：Monstro（int leftForward， int leftBackward， int leftSpeedPin，

int rightForward， int rightBackward， int rightSpeedPin，

int trigA， int echoA， int trigB， int echoB）

{

}

// Behavior

void Monstro：：Initialize（） {

initializeIMU（）;

}

bool Monstro：：Update（） {

readIMU（）;

}

// IMU

void Monstro：：initializeIMU（） {

_bno = Adafruit_BNO055（55）;

if （！_bno.begin（））

{

Serial.print（“No BNO055 detected”）;

while （1）;

}

delay（1000）;

_bno.setExtCrystalUse（true）;

}

void Monstro：：readIMU（） {

sensors_event_t event;

_bno.getEvent（&event）;

xTilt = event.orientation.x;

yTilt = event.orientation.y;

zTilt = event.orientation.z;

}

我們現(xiàn)在已經(jīng)實現(xiàn)了我們的IMU功能：

我們在我們的主Initialize（）函數(shù)中包含了IMU初始化，并在我們的主Update（）函數(shù)中包含了IMU讀取。

我們還實現(xiàn)了IMU初始化的代碼，我們與BNO055進行了接口

最后在readIMU（）函數(shù)內(nèi)部實現(xiàn)了機器人絕對定位的讀取。將三個傾斜分配給我們的內(nèi)部變量。

步驟6：電機控制

實現(xiàn)電機代碼控制將涉及比IMU代碼更多的邏輯。這是因為它將從我們稍后將在本教程中編寫的PID算法接收其值。

所以讓我們從更新頭文件開始：

#ifndef Monstro_h

#define Monstro_h

#include “Arduino.h”

#include

#include

#include

class Monstro {

public：

Monstro（int leftForward， int leftBackward， int leftSpeedPin，

int rightForward， int rightBackward， int rightSpeedPin，

int trigA， int echoA， int trigB， int echoB）;

// Behavior

bool Update（）;

void Initialize（）;

// IMU

volatile double xTilt;

volatile double yTilt;

volatile double zTilt;

private：

// IMU

Adafruit_BNO055 _bno;

void initializeIMU（）;

void readIMU（）;

// Motors

int _leftForward;

int _leftBackward;

int _leftSpeedPin;

int _rightForward;

int _rightBackward;

int _rightSpeedPin ;

void initializeMotors（）;

void setMotors（int leftMotorSpeed， int rightMotorSpeed）;

};

#endif

新行位于頭文件的底部，位于注釋“Motors”下。我們定義的私有變量將引用每個電機控制的引腳（方向引腳和速度引腳）。我們還包括兩個功能，一個用于初始化電機，另一個用于實際更改電機的速度和方向。

現(xiàn)在讓我們更新源文件以反映這些變化：

#include “Arduino.h”

#include “monstro.h”

Monstro：：Monstro（int leftForward， int leftBackward， int leftSpeedPin，

int rightForward， int rightBackward， int rightSpeedPin，

int trigA， int echoA， int trigB， int echoB）

{

_leftForward = leftForward;

_leftBackward = leftBackward;

_leftSpeedPin = leftSpeedPin;

_rightForward = rightForward;

_rightBackward = rightBackward;

_rightSpeedPin = rightSpeedPin;

}

// Behavior

void Monstro：：Initialize（） {

initializeIMU（）;

initializeMotors（）;

}

bool Monstro：：Update（） {

readIMU（）;

}

// IMU

void Monstro：：initializeIMU（） {

_bno = Adafruit_BNO055（55）;

if （！_bno.begin（））

{

Serial.print（“No BNO055 detected”）;

while （1）;

}

delay（1000）;

_bno.setExtCrystalUse（true）;

}

void Monstro：：readIMU（） {

sensors_event_t event;

_bno.getEvent（&event）;

xTilt = event.orientation.x;

yTilt = event.orientation.y;

zTilt = event.orientation.z;

}

// Motors

void Monstro：：initializeMotors（） {

pinMode（_leftForward， OUTPUT）;

pinMode（_leftBackward， OUTPUT）;

pinMode（_leftSpeedPin， OUTPUT）;

pinMode（_rightForward， OUTPUT）;

pinMode（_rightBackward， OUTPUT）;

pinMode（_rightSpeedPin， OUTPUT）;

}

void Monstro：：setMotors（int leftMotorSpeed， int rightMotorSpeed） {

if （rightMotorSpeed 《= 0） {

digitalWrite（_rightBackward， LOW）;

digitalWrite（_rightForward， HIGH）;

analogWrite（_rightSpeedPin， abs（rightMotorSpeed））;

}

else {

digitalWrite（_rightBackward， HIGH）;

digitalWrite（_rightForward， LOW）;

analogWrite（_rightSpeedPin， rightMotorSpeed）;

}

if （leftMotorSpeed 《= 0） {

digitalWrite（_leftBackward， LOW）;

digitalWrite（_leftForward， HIGH）;

analogWrite（_leftSpeedPin， abs（leftMotorSpeed））;

}

else {

digitalWrite（_leftBackward， HIGH）;

digitalWrite（_leftForward， LOW）;

analogWrite（_leftSpeedPin， leftMotorSpeed）;

}

}

更新如下：

我們現(xiàn)在已經(jīng)在構(gòu)造函數(shù)中分配了私有pin變量值，因此用戶可以根據(jù)特殊設(shè)置。

我們已將電機初始化添加到一般的Initialize（）函數(shù)中。

我們已經(jīng)實現(xiàn)了電機初始化程序，其中包括將引腳設(shè)置為輸出。

我們已經(jīng)定義了我們的功能，它將實際啟動電機setMotors（）。根據(jù)傳遞給該功能的值（-255至255），電機將以不同的速度和不同的方向開始旋轉(zhuǎn)。這些值將由PID算法在下一節(jié)中生成。

步驟7：PID算法實現(xiàn)

現(xiàn)在我將介紹更復(fù)雜的代碼部分：PID控制器算法。

這種算法用于許多自動控制應(yīng)用程序。它可以調(diào)節(jié)各種過程，從流量和溫度到調(diào)平和速度。基本上它是一個封閉的反饋循環(huán)，它接受一個變量作為 輸入 并在嘗試中產(chǎn)生 輸出 將 輸入 驅(qū)動到特定的 設(shè)定點 。

PID代表比例，積分和微分。這些術(shù)語中的每一個都以不同方式影響控制器響應(yīng)。它們將產(chǎn)生一個輸出，驅(qū)動我們的電機以保持我們的機器人平衡。

比例是控制器中的主要驅(qū)動術(shù)語。它會根據(jù)誤差（在我們的例子中是測量角度和所需角度之間的差異）改變控制器輸出。如果誤差變大，那么該項的增益將按比例增加。

積分術(shù)語會根據(jù)錯誤隨時間的累積影響我們的機器人對錯誤的響應(yīng)。如果在給定時間段內(nèi)誤差很大，則增加/減少將以快速速率發(fā)生。同樣，如果誤差很長一段時間，則變化將以較慢的速度發(fā)生。您可以將此視為基于系統(tǒng)過去行為的響應(yīng)。

派生術(shù)語根據(jù)錯誤的變化率生成輸出。這轉(zhuǎn)換為當(dāng)前誤差與先前誤差之差除以采樣周期。這個術(shù)語將有助于預(yù)測機器人的平衡在下一次閱讀中的反應(yīng)。您可以將此術(shù)語視為系統(tǒng)將來如何表現(xiàn)的預(yù)測性響應(yīng)。

因此，既然我們已經(jīng)基本了解了PID控制器在理論上的工作原理，那么我們就去吧提前并將其實施到我們的課堂中。我們可以從更新標(biāo)題開始：

#ifndef Monstro_h

#define Monstro_h

#include “Arduino.h”

#include

#include

#include

class Monstro {

public：

Monstro（int leftForward， int leftBackward， int leftSpeedPin，

int rightForward， int rightBackward， int rightSpeedPin，

int trigA， int echoA， int trigB， int echoB）;

// Behavior

bool Update（）;

void Initialize（）;

void ComputeBalance（）;

// IMU

volatile double xTilt;

volatile double yTilt;

volatile double zTilt;

private：

// IMU

Adafruit_BNO055 _bno;

void initializeIMU（）;

void readIMU（）;

// Motors

int _leftForward;

int _leftBackward;

int _leftSpeedPin;

volatile int _leftSpeed = 0;

int _rightForward;

int _rightBackward;

int _rightSpeedPin ;

volatile int _rightSpeed = 0;

void initializeMotors（）;

void setMotors（int leftMotorSpeed， int rightMotorSpeed）;

// PID

volatile float previous_error = 0， integral = 0;

volatile int motorPower;

float sampleTime = 0.005;

double outMin， outMax;

double _Kp， _Ki， _Kd;

volatile float Setpoint = 0， Input， Output;

void initializePID（）;

void SetTunings（double Kp， double Ki， double Kd）;

void SetOutputLimits（double Min， double Max）;

};

#endif

這里有很多新代碼，大部分內(nèi)容乍一看都難以理解，所以我會詳細說明：

盡可能看到我們在我們的類中添加了另一個公共函數(shù)：ComputeBalance（）。該功能將在定時器中斷中調(diào)用，并由我們的PID控制器算法組成。

我們還包含了一些我們將在實際實現(xiàn)中使用的變量，比如previous_error，以及我們需要在迭代之間存儲的積分。

在主Update（）循環(huán)中調(diào)用setMotors（）函數(shù)時，motorPower將用于驅(qū)動電機。

sampleTime是我們在幾秒鐘內(nèi)調(diào)用ComputeBalance函數(shù)的頻率。

變量outMin和outMax將幫助我們將輸出約束到我們的電機能夠讀取的值（在我們的例子中，這些值將是-255到255，但是在某些情況下我們可能需要更改這些值。

_Kp，_Ki和_Kd是我們的比例，積分和微分常數(shù)。這些算法的每個部分都會乘以。

設(shè)定點是我們想要的角度，如果我們的機器人想要保持平衡，它應(yīng)該設(shè)置為0.輸入是我們將從IMU的傾斜中讀取的，輸出是PID算法將給我們的。

我們還有一個初始化函數(shù)，以及另外兩個函數(shù)來幫助我們調(diào)整算法。

現(xiàn)在讓我們進入PID控制器的源代碼實現(xiàn)。這部分完全沒有完成，我們已經(jīng)寫好了這個算法的一些變體，但目前這個版本似乎在我們當(dāng)前的設(shè)置中效果最好：

#include “Arduino.h”

#include “monstro.h”

Monstro：：Monstro（int leftForward， int leftBackward， int leftSpeedPin，

int rightForward， int rightBackward， int rightSpeedPin，

int trigA， int echoA， int trigB， int echoB）

{

leftForward = leftForward;

_leftBackward = leftBackward;

_leftSpeedPin = leftSpeedPin;

_rightForward = rightForward;

_rightBackward = rightBackward;

_rightSpeedPin = rightSpeedPin;

}

// Behavior

void Monstro：：Initialize（） {

initializeIMU（）;

initializeMotors（）;

initializePID（）;

}

bool Monstro：：Update（） {

readIMU（）;

setMotors（motorPower， motorPower）;

}

// IMU

void Monstro：：initializeIMU（） {

_bno = Adafruit_BNO055（55）;

if （！_bno.begin（））

{

Serial.print（“No BNO055 detected”）;

while （1）;

}

delay（1000）;

_bno.setExtCrystalUse（true）;

}

void Monstro：：readIMU（） {

sensors_event_t event;

_bno.getEvent（&event）;

xTilt = event.orientation.x;

yTilt = event.orientation.y;

zTilt = event.orientation.z;

}

// Motors

void Monstro：：initializeMotors（） {

pinMode（_leftForward， OUTPUT）;

pinMode（_leftBackward， OUTPUT）;

pinMode（_leftSpeedPin， OUTPUT）;

pinMode（_rightForward， OUTPUT）;

pinMode（_rightBackward， OUTPUT）;

pinMode（_rightSpeedPin， OUTPUT）;

}

void Monstro：：setMotors（int leftMotorSpeed， int rightMotorSpeed） {

if （rightMotorSpeed 《= 0） {

digitalWrite（_rightBackward， LOW）;

digitalWrite（_rightForward， HIGH）;

analogWrite（_rightSpeedPin， abs（rightMotorSpeed））;

}

else {

digitalWrite（_rightBackward， HIGH）;

digitalWrite（_rightForward， LOW）;

analogWrite（_rightSpeedPin， rightMotorSpeed）;

}

if （leftMotorSpeed 《= 0） {

digitalWrite（_leftBackward， LOW）;

digitalWrite（_leftForward， HIGH）;

analogWrite（_leftSpeedPin， abs（leftMotorSpeed））;

}

else {

digitalWrite（_leftBackward， HIGH）;

digitalWrite（_leftForward， LOW）;

analogWrite（_leftSpeedPin， leftMotorSpeed）;

}

}

// PID

void Monstro：：initializePID（） {

SetOutputLimits（-250， 250）;

SetTunings（25， 0.5， 275）;

}

void Monstro：：ComputeBalance（） {

Input = zTilt;

// Compute error variables

float error = Input - Setpoint;

// Calculate proportional component

float proportional = error * _Kp;

// Calculate integral component

integral += error * _Ki;

integral = constrain（integral， outMin， outMax）; // limit wind-up

// Calculate derivative component

float derivative = （error - previous_error） * _Kd;

// Save variables for next error computation

previous_error = error;

// Add up PID

Output = proportional + integral + derivative;

// Limit to PWM constraints

Output = constrain（Output， outMin， outMax）;

// Motor control

motorPower = Output;

// give up if there is no chance of success

if （Input 《 -40 || Input 》 40） motorPower = 0;

}

void Monstro：：SetTunings（double Kp， double Ki， double Kd） {

_Kp = Kp;

_Ki = Ki;

_Kd = Kd;

}

void Monstro：：SetOutputLimits（double Min， double Max） {

if （Min 》 Max） return;

outMin = Min;

outMax = Max;

}

讓我們回顧一下我們的變化：

我們包括了Update（）循環(huán)內(nèi)部的setMotors（）函數(shù)。這將確保每次主回路運行時我們的電機旋轉(zhuǎn)，并根據(jù)PID控制器（motorPower）提供的輸出更新速度。

initializePID（）函數(shù)被添加到我們的主Initialize（）函數(shù)中。它用于設(shè)置常量，并設(shè)置我們的最小和最大輸出。

ComputeBalance（）函數(shù)本身就是PID計算發(fā)生的地方。它首先將我們機器人的zTilt作為輸入。然后我們通過檢查輸入和設(shè)定點之間的差異來計算誤差（如果我們保持機器人平衡，則應(yīng)該為0）。然后，我們通過將它與誤差相乘來計算比例項。接下來將誤差*積分常數(shù)加到我們的積分項中，并將其約束到我們的最大值和最小值，以限制此項可能導(dǎo)致的上升（如果我們的機器人跌落了一段時間，我們想要將其恢復(fù)原狀沒有它表現(xiàn)得很瘋狂）。然后通過檢查當(dāng)前誤差與我們上次測量誤差之間的差異，并將其乘以導(dǎo)數(shù)常數(shù)來計算導(dǎo)數(shù)。然后，我們將當(dāng)前錯誤保存為最后一個錯誤，并將我們的術(shù)語加在一起以計算輸出。此輸出現(xiàn)在可以分配給motorPower，它將被讀取以在主Update（）循環(huán)中驅(qū)動我們的電機。最后但并非最不重要的是，我們還希望確保將motorPower轉(zhuǎn)為0，以防我們的機器人傾斜超出可以恢復(fù)的程度，這樣當(dāng)它跌落時它不會繼續(xù)旋轉(zhuǎn)車輪并自行毀壞。

步驟8：調(diào)整PID常量

有一些已建立的調(diào)整PID常數(shù)的數(shù)學(xué)策略，如Ziegler- Nichols方法，或Cohen-Coon方法。但是，我們發(fā)現(xiàn)很難在我們的系統(tǒng)中實現(xiàn)這些方法，因此選擇了一些更簡單的規(guī)則進行調(diào)優(yōu)：

將所有常量設(shè)置為零。然后慢慢增加_Kp，直到機器人開始振蕩。如果傾斜到一側(cè)，即使它落到另一側(cè)，也要確保它始終是正確的。

定期增加_Kd，直到您注意到振蕩開始減少。

增加_Ki，以便在機器人真正失去平衡時響應(yīng)更快，而在距離設(shè)定點稍微偏離時更慢。這可以改善增加_Kd時減少的反應(yīng)時間。

從這一點微調(diào)常數(shù)，直到機器人可以無限期地保持其平衡。

上傳到本節(jié)開頭的gif也可以作為視覺指南。至于這些常數(shù)的效果。我們發(fā)現(xiàn)它作為一種可視化調(diào)整工具非常有用。

步驟9：超聲波傳感器

超聲波傳感器代碼是特別是每個設(shè)置和錯誤類型。因此，我們不會將其包含在這個教學(xué)中。但是，對于所有類型的運動，整體邏輯是相同的：將Setpoint變量更改為大于0，機器人將以一種方式行進，將其更改為低于0，機器人將以另一種方式行進。您還可以將常數(shù)乘以每個車輪的速度，以使機器人左右轉(zhuǎn)動。

（更新：進一步考慮后，本節(jié)將很快詳細說明）

步驟10：使用Monster庫

現(xiàn)在我們已經(jīng)對所有編碼的庫進行了編碼，我們可以在簡單的Arduino草圖中使用它。

我們將通過導(dǎo)入庫頭來實現(xiàn)，構(gòu)造Monster類的一個實例，并使用Timer Interrupt（來自TimerOne.h庫）定期調(diào)用ComputeBalance（）函數(shù)。

實施代碼如下：

#include “monstro.h”

#include

Monstro meuMonstro（13， 12， 11， 3， 5， 6， 13， 12， 8， 7）;

void setup（）

{

// COM

Serial.begin（9600）;

// Timer Interrupt

Timer1.initialize（5000）;

Timer1.attachInterrupt（BalanceRobot）;

meuMonstro.Initialize（）;

}

void loop（）

{

meuMonstro.Update（）;

}

void BalanceRobot（） {

meuMonstro.ComputeBalance（）;

}

將此上傳到您的Arduino，將公桶插孔插入機器人母筒插孔供電，機器人應(yīng)開始自行平衡。