第1步：理論

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

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

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

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

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

機(jī)箱：

將膠合板切成3塊，每塊寬9厘米，長(zhǎng)14.5厘米，這些將成為機(jī)器人底盤的三個(gè)平臺(tái)。

在每個(gè)平臺(tái)的角落鉆6mm孔，距離邊緣10mm（見圖）。

標(biāo)記三個(gè)平臺(tái)：頂部，中部和底部。

在頂級(jí)平臺(tái)上鉆出Arduino Uno和L298N驅(qū)動(dòng)板的孔。

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

安裝在底部平臺(tái)上的電機(jī)的鉆孔，以及電機(jī)上的電線孔（見圖）。

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

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

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

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

縱向切割其中一個(gè)廚房海綿，并將每個(gè)橡膠到頂部平臺(tái)的兩半作為保險(xiǎn)杠。此步驟僅用于測(cè)試。

軸：

在您的中心鉆一個(gè)6毫米的洞車輪。

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

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

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

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

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

將電機(jī)穿過底部平臺(tái)的中心孔。

控制中心：

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

激光切割出2mm有機(jī)玻璃的文件。

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

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

將控制中心放在海綿上。

擠壓海綿，將兩塊小木塊（約15x15毫米）放在控制中心和中間平臺(tái)之間。它是一種易于拆卸的方式來放入和放出我們的控制箱。我們?cè)谏厦娴膱D片上實(shí)現(xiàn)。

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

電機(jī)驅(qū)動(dòng)器

L298N的使能引腳用于控制使用PWM（脈沖寬度調(diào)制）的電機(jī)速度，而驅(qū)動(dòng)器的In1-4引腳用于切換電機(jī)的方向。以下是描述本節(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ū)動(dòng)器的電源。

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

將其中一個(gè)電機(jī)的正負(fù)電源線連接到MotorA螺絲端子。

將其他電機(jī)的正負(fù)電源線連接到MotorB螺絲端子。

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

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

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

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

在迷你面包板上放置另一個(gè)螺絲端子，將我們之前放入L298N的GND引腳的另一端連接到此端子。確保它也連接到我們?cè)谏弦徊街薪⒌牡鼐€。我們的接地電路現(xiàn)在應(yīng)該完整了。 （如果這部分令人困惑，請(qǐng)查看圖像。）

BNO055絕對(duì)定向傳感器

BNO055是一款9自由度傳感器。它將來自加速度計(jì)，陀螺儀和磁力計(jì)的數(shù)據(jù)融合到絕對(duì)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)。

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

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

HC-SR04超聲波傳感器

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

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

電源

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

切出5米來自電源線軸的長(zhǎng)鏈。

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

裝配

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

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

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

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

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

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

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

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

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

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

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

我們將使用Update（）函數(shù)在主循環(huán)期間調(diào)用某些行為，并且Initialize（）函數(shù)確保我們的傳感器和電機(jī)準(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（） {

}

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

步驟5：測(cè)量?jī)A角（IMU）

由于Adafruit程序員編寫的庫，實(shí)現(xiàn)BNO055的代碼非常簡(jiǎn)單。我們將使用Adafruit_BNO055驅(qū)動(dòng)程序庫以及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

我們?cè)陬^文件中添加了一些內(nèi)容。

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

我們還添加了公共變量xTilt，yTilt和zTilt。這些是我們將在每個(gè)更新周期中存儲(chǔ)從IMU檢索的數(shù)據(jù)的地方。請(qǐng)注意，我們已將它們標(biāo)記為volatile，這是因?yàn)槲覀儗⒃诒窘坛毯竺娴挠?jì)時(shí)器中斷中使用它們。

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

現(xiàn)在讓我們?cè)谠次募袑?shí)現(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)實(shí)現(xiàn)了我們的IMU功能：

我們?cè)谖覀兊闹鱅nitialize（）函數(shù)中包含了IMU初始化，并在我們的主Update（）函數(shù)中包含了IMU讀取。

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

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

步驟6：電機(jī)控制

實(shí)現(xiàn)電機(jī)代碼控制將涉及比IMU代碼更多的邏輯。這是因?yàn)樗鼘奈覀兩院髮⒃诒窘坛讨芯帉懙腜ID算法接收其值。

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

#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”下。我們定義的私有變量將引用每個(gè)電機(jī)控制的引腳（方向引腳和速度引腳）。我們還包括兩個(gè)功能，一個(gè)用于初始化電機(jī)，另一個(gè)用于實(shí)際更改電機(jī)的速度和方向。

現(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è)置。

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

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

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

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

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

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

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

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

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

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

因此，既然我們已經(jīng)基本了解了PID控制器在理論上的工作原理，那么我們就去吧提前并將其實(shí)施到我們的課堂中。我們可以從更新標(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)容乍一看都難以理解，所以我會(huì)詳細(xì)說明：

盡可能看到我們?cè)谖覀兊念愔刑砑恿肆硪粋€(gè)公共函數(shù)：ComputeBalance（）。該功能將在定時(shí)器中斷中調(diào)用，并由我們的PID控制器算法組成。

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

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

sampleTime是我們?cè)趲酌腌妰?nèi)調(diào)用ComputeBalance函數(shù)的頻率。

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

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

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

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

現(xiàn)在讓我們進(jìn)入PID控制器的源代碼實(shí)現(xiàn)。這部分完全沒有完成，我們已經(jīng)寫好了這個(gè)算法的一些變體，但目前這個(gè)版本似乎在我們當(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ù)。這將確保每次主回路運(yùn)行時(shí)我們的電機(jī)旋轉(zhuǎn)，并根據(jù)PID控制器（motorPower）提供的輸出更新速度。

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

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

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

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

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

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

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

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

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

步驟9：超聲波傳感器

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

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

步驟10：使用Monster庫

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

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

實(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，將公桶插孔插入機(jī)器人母筒插孔供電，機(jī)器人應(yīng)開始自行平衡。