自平衡机器人的制作教程

第1步：理论

自平衡机器人的问题是倒立摆的问题。为了抵消机器人向前或向后下落的力，我们需要一种机制，使其重心直接保持在其枢转点的上方。这个枢轴点将是我们的轮轴。我们的反作用策略将通过驱动机器人的车轮沿其下降的方向进行。

然而，问题是停在那里。如果我们有一个简单的反馈回路来检查机器人正朝着哪个方向下降并沿着那个方向驱动车轮，那么我们的机器人将会固有地振荡并崩溃。

因此我们的战略将涉及实施PID控制器来驱动车轮以受控的数学方式来回，响应机器人下落的方向，它下降的速度，到目前为止倾斜的量，以及所有这三个变量之间的关系。

有关如何实现的细节将在本教程的PID部分进一步说明。

步骤2：构建轴和控制中心的Chasis

机箱：

将胶合板切成3块，每块宽9厘米，长14.5厘米，这些将成为机器人底盘的三个平台。

在每个平台的角落钻6mm孔，距离边缘10mm（见图）。

标记三个平台：顶部，中部和底部。

在顶级平台上钻出Arduino Uno和L298N驱动板的孔。

测量中间平台的中心点，并标出小型面包板的区域以及用于连接器的位置（参见图表）。

安装在底部平台上的电机的钻孔，以及电机上的电线孔（见图）。

将M6螺纹杆切成四个25厘米的小块。

将每个部件滑入平台的相应孔中，用M6垫圈和M6螺母固定每个孔的两侧。

您的平台应相距约9厘米，剩余长度应从顶部平台伸出。

测量每个平台的倾斜角度，并调整每个螺母，使其全部与地面齐平。

纵向切割其中一个厨房海绵，并将每个橡胶到顶部平台的两半作为保险杠。此步骤仅用于测试。

轴：

在您的中心钻一个6毫米的洞车轮。

将30毫米M6螺纹杆滑入孔中。用M6垫圈和M6螺母固定外端，并用另一个M6螺母固定内端。

将联轴器安装到M6螺纹杆的内端，并拧紧固定螺钉将其固定到位。

将耦合器的开口端连接到电机轴上，并拧紧固定螺钉将其固定到位。确保两个轮子与电机本身的距离相同。

从卷筒上切下两根40厘米长的电源线，然后将它们焊接到电机的端子上。

拿起5毫米木螺钉，按照底盘构造阶段制作的导孔，将电机连接到底部平台。

将电机穿过底部平台的中心孔。

控制中心：

下载本节顶部的dxf文件（bug lounge cut file.dxf）。

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

根据本节顶部的图表组装零件。

将厨房海绵放在底部平台上（如果需要，可以用胶水或双层胶带）。

将控制中心放在海绵上。

挤压海绵，将两块小木块（约15x15毫米）放在控制中心和中间平台之间。它是一种易于拆卸的方式来放入和放出我们的控制箱。我们在上面的图片上实现。

第3步：构建电路

电机驱动器

L298N的使能引脚用于控制使用PWM（脉冲宽度调制）的电机速度，而驱动器的In1-4引脚用于切换电机的方向。以下是描述本节顶部Fritzing图的说明。

将L298N的EnA引脚连接到Arduino的数字引脚6.

将In1引脚连接到数字引脚5，将In2引脚连接到数字引脚3.

将L298N的EnB引脚连接到Arduino的数字引脚11.

将In3引脚连接到数字引脚13，将In4引脚连接到数字引脚12.

取下5V_EN跳线，以便为Arduino提供驱动器的电源。

将5V螺丝端子从L298N连接到Arduino的Vin引脚。

将其中一个电机的正负电源线连接到MotorA螺丝端子。

将其他电机的正负电源线连接到MotorB螺丝端子。

切掉另一根电源线，将红线连接到L298N的VMS引脚，黑线连接到L298N的GND引脚。红线的另一端应连接到Wago连接器。

将螺丝端子放在迷你面包板的末端。

切掉另一根电源线并将其连接到母筒插孔。然后红色端应连接到Wago连接器，以完成电路一直到L298N的VMS引脚，而黑色端将进入我们之前放入迷你面包板的螺丝端子。

将Arduino的GND引脚连接到与迷你面包板中的母筒插孔相同的线路中。这将确保我们的系统基础全部连接。

在迷你面包板上放置另一个螺丝端子，将我们之前放入L298N的GND引脚的另一端连接到此端子。确保它也连接到我们在上一步中建立的地线。我们的接地电路现在应该完整了。 （如果这部分令人困惑，请查看图像。）

BNO055绝对定向传感器

BNO055是一款9自由度传感器。它将来自加速度计，陀螺仪和磁力计的数据融合到绝对3D方向。 BNO055使用I2C通信，因此我们将它连接到Arduino Uno的A5和A4引脚。这将根据您选择使用的Arduino的类型而改变。

将标题条焊接到IMU的分线板中。

将IMU放在迷你面包板上。

使用跨接电缆将Arduino的5V引脚连接到迷你面包板。

将IMU的Vin引脚与来自迷你面包板上的Arduino的5V电缆串联。

将IMU的GND引脚与来自迷你面包板上的Arduino的GND引脚串联。

用更长的跨接电缆将其从IMU的SCL引脚连接到Arduino的A5引脚（它兼作SCL引脚）。

再用一根长跨接电缆将其从IMU的SDA引脚连接到Arduino的A4引脚（兼作SDA引脚）。

HC-SR04超声波传感器

HC-SR04传感器是超声波测距模块，提供2cm至400cm的测量功能，精度为3mm。它的工作原理是发送脉冲，并检测接收脉冲所需的时间。通过该脉冲测量的距离可以分解为一个简单的等式：距离=（高水平时间*声速）/2

将HC-SR04的VCC引脚与来自迷你面包板上的Arduino的5V电缆串联。

将HC-SR04的GND引脚与来自迷你面包板上Arduino的GND电缆串联。

将HC-SR04的Trig引脚连接到Arduino的Digital 4引脚。

将HC-SR04的Echo引脚连接到Arduino的Digital 2引脚。

使用第二个HC-SR04重复步骤1到4，但这次使用数字引脚7作为Trig，数字引脚8作为Echo。

电源

O 你的电机需要12V和每个约2安培，所以我们将使用外部电源来提供这种电力。 arduino本身将由电机驱动器的5V输出供电。

切出5米来自电源线轴的长链。

剥去两侧的两端。将电源螺丝端子的另一端连接到另一端。

装配

将电子装配到机箱很简单。只需按照您在机箱构造步骤中制作的导孔即可。

使用5mm木螺钉并使用塑料上的安装孔将Arduino连接到顶部平台案件。

取7mm垫片，将它们放在L298N电机驱动器下方，然后将M4螺栓穿过安装孔并穿过垫片。

迷你面包板下面应该有一块双面胶带。取下此贴片的覆盖物，将迷你面包板粘在中间平台的中央。确保IMU位于平台的中心，您可能需要调整面包板才能这样做。

取另一块双面胶带，将Wago连接器连接到中间平台的边缘。

使用扎带将阴筒固定在其中一根螺杆上。

出于测试目的，将清洁海绵切成两半并将每一半连接到顶部平台的两侧，使用橡皮筋将其固定到位。您可以在机器人独立后立即将其移除，但在此之前，这将使我们的电子设备免受损坏。

第4步：编码：设置怪物类

为了以一种易于被其他开发者构建的方式编程我们的Monster，我们将它作为一个类实现/图书馆。一个类由头文件（.h）和源文件（.cpp）组成。头文件定义了类中的所有内容，而源文件包含实际的代码实现。

我们将从头文件开始：

#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

我们在这里所做的就是设置带有构造函数的头文件，该构造函数接收我们将用于与我们的传感器和驱动程序交互的引脚。稍后我们将在介绍每个组成部分时添加此内容。

#include Arduino.h语句只是确保我们可以访问Arduino语言提供的常量和类型。

我们将使用Update（）函数在主循环期间调用某些行为，并且Initialize（）函数确保我们的传感器和电机准备就绪。在后面的步骤中有更多相关内容。

我们的源文件将反映此头文件：

#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（） {

}

再次，我们在这里所做的只是设置裸源文件的骨骼，同时确保我们在这里也包含Arduino.h引用，以及对头文件的引用，以便我们也可以访问它的定义。

步骤5：测量倾角（IMU）

由于Adafruit程序员编写的库，实现BNO055的代码非常简单。我们将使用Adafruit_BNO055驱动程序库以及Adafruit统一传感器库。

让我们首先更新我们的头文件以与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

我们在头文件中添加了一些内容。

首先，你会注意到三个include语句，它们确保我们可以访问Adafruit库以及imumaths.h库，它们在实现IMU读取时将需要它们的功能。

我们还添加了公共变量xTilt，yTilt和zTilt。这些是我们将在每个更新周期中存储从IMU检索的数据的地方。请注意，我们已将它们标记为volatile，这是因为我们将在本教程后面的计时器中断中使用它们。

我们还添加了一个BNO055对象（_bno），一个初始化函数来设置它，以及一个在更新周期中使用的读取函数。

现在让我们在源文件中实现这些功能：

#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;

}

我们现在已经实现了我们的IMU功能：

我们在我们的主Initialize（）函数中包含了IMU初始化，并在我们的主Update（）函数中包含了IMU读取。

我们还实现了IMU初始化的代码，我们与BNO055进行了接口

最后在readIMU（）函数内部实现了机器人绝对定位的读取。将三个倾斜分配给我们的内部变量。

步骤6：电机控制

实现电机代码控制将涉及比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”下。我们定义的私有变量将引用每个电机控制的引脚（方向引脚和速度引脚）。我们还包括两个功能，一个用于初始化电机，另一个用于实际更改电机的速度和方向。

现在让我们更新源文件以反映这些变化：

#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）;

}

}

更新如下：

我们现在已经在构造函数中分配了私有pin变量值，因此用户可以根据特殊设置。

我们已将电机初始化添加到一般的Initialize（）函数中。

我们已经实现了电机初始化程序，其中包括将引脚设置为输出。

我们已经定义了我们的功能，它将实际启动电机setMotors（）。根据传递给该功能的值（-255至255），电机将以不同的速度和不同的方向开始旋转。这些值将由PID算法在下一节中生成。

步骤7：PID算法实现

现在我将介绍更复杂的代码部分：PID控制器算法。

这种算法用于许多自动控制应用程序。它可以调节各种过程，从流量和温度到调平和速度。基本上它是一个封闭的反馈循环，它接受一个变量作为 输入 并在尝试中产生 输出 将 输入 驱动到特定的 设定点 。

PID代表比例，积分和微分。这些术语中的每一个都以不同方式影响控制器响应。它们将产生一个输出，驱动我们的电机以保持我们的机器人平衡。

比例是控制器中的主要驱动术语。它会根据误差（在我们的例子中是测量角度和所需角度之间的差异）改变控制器输出。如果误差变大，那么该项的增益将按比例增加。

积分术语会根据错误随时间的累积影响我们的机器人对错误的响应。如果在给定时间段内误差很大，则增加/减少将以快速速率发生。同样，如果误差很长一段时间，则变化将以较慢的速度发生。您可以将此视为基于系统过去行为的响应。

派生术语根据错误的变化率生成输出。这转换为当前误差与先前误差之差除以采样周期。这个术语将有助于预测机器人的平衡在下一次阅读中的反应。您可以将此术语视为系统将来如何表现的预测性响应。

因此，既然我们已经基本了解了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（）;

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

这里有很多新代码，大部分内容乍一看都难以理解，所以我会详细说明：

尽可能看到我们在我们的类中添加了另一个公共函数：ComputeBalance（）。该功能将在定时器中断中调用，并由我们的PID控制器算法组成。

我们还包含了一些我们将在实际实现中使用的变量，比如previous_error，以及我们需要在迭代之间存储的积分。

在主Update（）循环中调用setMotors（）函数时，motorPower将用于驱动电机。

sampleTime是我们在几秒钟内调用ComputeBalance函数的频率。

变量outMin和outMax将帮助我们将输出约束到我们的电机能够读取的值（在我们的例子中，这些值将是-255到255，但是在某些情况下我们可能需要更改这些值。

_Kp，_Ki和_Kd是我们的比例，积分和微分常数。这些算法的每个部分都会乘以。

设定点是我们想要的角度，如果我们的机器人想要保持平衡，它应该设置为0.输入是我们将从IMU的倾斜中读取的，输出是PID算法将给我们的。

我们还有一个初始化函数，以及另外两个函数来帮助我们调整算法。

现在让我们进入PID控制器的源代码实现。这部分完全没有完成，我们已经写好了这个算法的一些变体，但目前这个版本似乎在我们当前的设置中效果最好：

#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（）循环内部的setMotors（）函数。这将确保每次主回路运行时我们的电机旋转，并根据PID控制器（motorPower）提供的输出更新速度。

initializePID（）函数被添加到我们的主Initialize（）函数中。它用于设置常量，并设置我们的最小和最大输出。

ComputeBalance（）函数本身就是PID计算发生的地方。它首先将我们机器人的zTilt作为输入。然后我们通过检查输入和设定点之间的差异来计算误差（如果我们保持机器人平衡，则应该为0）。然后，我们通过将它与误差相乘来计算比例项。接下来将误差*积分常数加到我们的积分项中，并将其约束到我们的最大值和最小值，以限制此项可能导致的上升（如果我们的机器人跌落了一段时间，我们想要将其恢复原状没有它表现得很疯狂）。然后通过检查当前误差与我们上次测量误差之间的差异，并将其乘以导数常数来计算导数。然后，我们将当前错误保存为最后一个错误，并将我们的术语加在一起以计算输出。此输出现在可以分配给motorPower，它将被读取以在主Update（）循环中驱动我们的电机。最后但并非最不重要的是，我们还希望确保将motorPower转为0，以防我们的机器人倾斜超出可以恢复的程度，这样当它跌落时它不会继续旋转车轮并自行毁坏。

步骤8：调整PID常量

有一些已建立的调整PID常数的数学策略，如Ziegler- Nichols方法，或Cohen-Coon方法。但是，我们发现很难在我们的系统中实现这些方法，因此选择了一些更简单的规则进行调优：

将所有常量设置为零。然后慢慢增加_Kp，直到机器人开始振荡。如果倾斜到一侧，即使它落到另一侧，也要确保它始终是正确的。

定期增加_Kd，直到您注意到振荡开始减少。

增加_Ki，以便在机器人真正失去平衡时响应更快，而在距离设定点稍微偏离时更慢。这可以改善增加_Kd时减少的反应时间。

从这一点微调常数，直到机器人可以无限期地保持其平衡。

上传到本节开头的gif也可以作为视觉指南。至于这些常数的效果。我们发现它作为一种可视化调整工具非常有用。

步骤9：超声波传感器

超声波传感器代码是特别是每个设置和错误类型。因此，我们不会将其包含在这个教学中。但是，对于所有类型的运动，整体逻辑是相同的：将Setpoint变量更改为大于0，机器人将以一种方式行进，将其更改为低于0，机器人将以另一种方式行进。您还可以将常数乘以每个车轮的速度，以使机器人左右转动。

（更新：进一步考虑后，本节将很快详细说明）

步骤10：使用Monster库

现在我们已经对所有编码的库进行了编码，我们可以在简单的Arduino草图中使用它。

我们将通过导入库头来实现，构造Monster类的一个实例，并使用Timer Interrupt（来自TimerOne.h库）定期调用ComputeBalance（）函数。

实施代码如下：

#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，将公桶插孔插入机器人母筒插孔供电，机器人应开始自行平衡。