Line Follower Robot - PID 控制 - Android 设置

应用和在线服务

	Arduino IDE
	MIT App Inventor 2

关于这个项目

该项目的目的是构建一个带有 PID 控制的 Line Follower Robot。我们还将使用 Android 设备轻松设置主要控制参数，以便更好、更快速地进行调整。

这个项目是两个更复杂的项目中的第一个，我的目的是探索 Line Follower Robots 的潜力。第二部分：迷宫解谜机器人，使用人工智能和Arduino，机器人将使用简单的人工智能技术探索和解决迷宫。

下面是一段视频，显示机器人遵循线路电路：

第 1 步：物料清单

所需材料清单非常简单，最终机器人非常便宜（约 75.00 美元）：

身体（可根据您的需要进行调整）：

• 2 个木方块 (80X80mm)

• 3 个活页夹

• 2个木轮（直径：50mm）

• 1 个球型脚轮

• 9 条松紧带

• 3M 命令框条

• 用于传感器固定的塑料接头

• 面包板和接线

• 2 组 4XNi-金属氢化物电池（每组 5V）

• 2 X SM-S4303R 连续旋转 360 度塑料伺服

• Arduino Nano

• HC-06 蓝牙模块

• 5 个 X 线传感器（TCRT5000 4CH 红外线轨跟随器传感器模块 + 1 个独立的轨道传感器）

• 1 个 LED

• 1 个按钮

第 2 步：设置电机

对于电机，使用了 2 个连续伺服 (SM-S4303R)。它们将被“粘合”在一起，形成一个单一的实心块，如您在照片中所见（使用 3M 命令条、胶水或双面胶带）。这些伺服系统将以给定的速度运行，由其数据输入上接收到的脉冲宽度定义。对于这个特定的伺服器，脉冲宽度从 1.0 毫秒（1,000 微秒）到 2.0 毫秒（2,000 微秒）。其他舵机可以使用不同的脉冲宽度。

查看详情：

• 1.5ms 的脉冲会将伺服定位在中立位置，或“停止”。

• 1.0ms 的脉冲将命令伺服器在一个方向上全速（大约 70 RPM）

• 一个 2.0ms 全速的反方向脉冲。

• 1.0 到 1.5ms 或 1.5ms 到 2.0ms 之间的脉冲将产生成比例的速度。

将两个伺服器物理连接后，按照上面的绘图电路为它们供电（外部 5V 或 6V）并为它们提供 Arduino 的信号：

• 左舵机：Arduino Pin 5

• 右舵机：Arduino Pin 3

全部连接后，必须做的第一件事是发送 1.5ms 脉冲以验证电机是否“停止”（未运行）。如果不是，则必须将舵机调整到完全停止（寻找黄色螺栓，舵机下方）。

注意 ：如果你的舵机没有这个物理调整，试着改变函数内部的参数“1500”微秒（向上或向下），直到你得到句号。

下面的 Arduino 代码可以完成这项工作：

#include  // 伺服库 Servo leftServo;Servo rightServo;Void setup(){ leftServo.attach(5); rightServo.attach(3); leftServo.writeMicroseconds(1500); rightServo.writeMicroseconds(1500);}void loop(){} 

第 3 步：组装车身和电机以进行运动测试

• 使用 3M Command 框架条，将 2 个舵机固定到一个方形木块上。

• 使用活页夹将第二个方形木头固定在上面的木头上。根据您的需要调整平台的长度。

• 使用活页夹固定球脚轮。

• 电机的电源将来自 5V 电池组中的一个。这套电池将安装在面包板和车身框架之间。

• 连接舵机使用的电池：左侧横向电网的一个专供舵机源

• 将 Arduino Nano 连接到面包板

• 将电网的 GND 连接到 Arduino GND。

• 将舵机连接到 Arduino：左 ==> 引脚 5；右 ==> 引脚 3

• 将 LED 连接到 Arduino 引脚 13

• 将按钮连接到 Arduino Pin 9

请注意，由于伺服器的安装方式（相对），速度范围是：

• 右舵机前进速度从 1,500us（停止）到 2,000us（全速）

• 左舵机前进速度从 1,500us（停止）到 1,000（全速）

外部 LED 添加到 pin13，用于信号和测试目的（您可以使用内部 Arduino LED，如果需要，可以使用外部 LED，但要考虑到在电缆中间很难看到它）。

还有一个按钮连接到引脚 9。这个按钮对于测试目的和机器人启动非常有用。

例如：

while(digitalRead(buttonPin)) { }motorTurn (LEFT, 500);motorTurn (RIGHT, 500); 

请注意，命令机器人向左转、等待 500 毫秒和向右转的 2 行只会在您按下按钮（buttonPin =0）后发生。在此之前，程序将在无限循环中停止。

以下代码可用作完整电机测试（向前、向后、完全停止、向左转、向右转）的基础。如有必要，您必须根据您的电机调整所需转向角的延迟（此外，有时左右脉冲值应略有不同，以补偿电机的任何不平衡。

第四步：蓝牙模块（可选）

蓝牙模块 HC-06 应安装在面包板上，如图所示。将使用 Arduino 库 SoftSerial。

在 HC-06 引脚连接下方：

• Tx 引脚 到 Arduino 引脚 10 （接收）

• RX 引脚 到 Arduino 引脚 11 (Tx)

• VCC/GND 到 Arduino 5V/GND

机器人可以使用或不使用蓝牙。该代码的构建方式是，如果您不激活 BT，则默认参数将是机器人使用的参数。因此，如果您不想安装 HC-06 模块，请不要担心，代码仍然可以正常工作。在本教程的最后一部分，我将探讨如何使用 Android 应用程序发送数据以更好地调整机器人参数和/或在手动模式下移动机器人。如果有人想更多地探索使用 Line Follower Robot 进行比赛，我会将蓝牙和应用程序的使用作为可选。

第 5 步：添加线传感器

将电缆连接到 Arduino 引脚，如下所示：

• 传感器 0 =12

• 传感器 1 =18

• 传感器 2 =17

• 传感器 3 =16

• 传感器 4 =19

• 如图所示将 5 个传感器固定在塑料条上

• 建议在传感器上贴上标签以进行测试。传感器名称从“0”（更多向左）到“4”（更多向右）

• 在框架下铺设电缆，使用松紧带将其固定。注意不要与轮子或脚轮混淆。

• 固定第二组 5V 电池并将其连接到 Arduino Vin。

就我而言，我使用了一个集成了 4 个传感器 + 1 个额外传感器的模块。它们都是兼容的。为简单起见，我在图中包含了 5 个连接在一起的独立传感器。两种配置的最终结果相同。

第 6 步：实现红外传感器逻辑

IR 传感器由一个单独的 IR LED 和一个 IR 光电二极管组成。 LED 发出的红外光照射到表面并反射回红外光电二极管。然后，光电二极管产生与表面反射率水平成正比的输出电压（“亮表面”的值较高，“黑/暗表面”的值较低）。

在使用传感器的情况下，模块上的集成电路生成一个简单的数字信号作为输出（高：暗；低：亮）。安装在模块上的电位计（见照片）将调整正确的光线水平，使其被视为“暗”或“亮”。它的工作原理是，当反射光颜色为黑色/深色时，在其输出端生成一个 HIGH（“1”）数字电平，并为另一种较浅的颜色生成一个 LOW（“0”）数字电平。我在这里使用了一个带有 4 个传感器的集成模块和一个带有唯一传感器的额外模块（不同的形状，但相同的逻辑）。这种组合是由 5 个传感器组成的阵列，我发现它有利于实现良好而流畅的控制，如下所述。

5 个传感器阵列的安装方式是，如果只有一个传感器相对于黑线居中，则只有该特定传感器会产生 HIGH。另一方面，应计算传感器之间的空间，以允许 2 个传感器同时覆盖黑线的整个宽度，同时在两个传感器上产生 HIGH（见上图）。

跟随一行时可能的传感器阵列输出是：

• 0 0 0 0 1

• 0 0 0 1 1

• 0 0 0 1 0

• 0 0 1 1 0

• 0 0 1 0 0

• 0 1 1 0 0

• 0 1 0 0 0

• 1 1 0 0 0

• 1 0 0 0 0

拥有 5 个传感器，允许生成一个“误差变量”，这将有助于控制机器人在线上的位置，如下所示。

让我们考虑最佳条件是当机器人居中时，使线刚好在“中间传感器”（传感器 2）下方。数组的输出将是：0 0 1 0 0，在这种情况下，“错误”将是“零”。如果机器人开始向左行驶（线“似乎向右移动”），则误差必须随着正信号增加。如果机器人开始向右移动（线“似乎向左移动”），则在同理，误差一定会增加，但现在是负信号。

与传感器状态相关的错误变量为：

0 0 1 0 0 ==> 错误 =0

• 0 0 0 0 1 ==> 错误 =4

• 0 0 0 1 1 ==> 错误 =3

• 0 0 0 1 0 ==> 错误 =2

• 0 0 1 1 0 ==> 错误 =1

• 0 1 1 0 0 ==> 错误 =-1

• 0 1 0 0 0 ==> 错误 =-2

• 1 1 0 0 0 ==> 错误 =-3

• 1 0 0 0 0 ==> 错误 =-4

查看 Arduino 代码，每个传感器都将定义一个特定的名称（考虑到更靠左的 Line Follow Sensor 必须分配标签“0”）：

const int lineFollowSensor0 =12;const int lineFollowSensor1 =18;const int lineFollowSensor2 =17;const int lineFollowSensor3 =16;const int lineFollowSensor4 =19; 

为了存储每个传感器的值，将创建一个数组变量：

int LFSensor[5]={0, 0, 0, 0, 0}; 

阵列的每个位置都会随着每个传感器的输出不断更新：

LFSensor[0] =digitalRead(lineFollowSensor0);LFSensor[1] =digitalRead(lineFollowSensor1);LFSensor[2] =digitalRead(lineFollowSensor2);LFSensor[3] =digitalRead(lineFollowSensor3);LFSensor[4] =digitalRead(lineFollowSensor4); 

有了每个传感器的值，必须实现一个逻辑来生成错误变量：

if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor [4]==1 )) 错误 =4;else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3] ==1 )&&(LFSensor[4]==1 )) 错误 =3; else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==1 )&&(LFSensor[4]==0 )) 错误 =2; 否则 if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==1 )&&(LFSensor[3]==1 )&&(LFSensor[4]==0 )) error =1;else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==1 )&&(LFSensor[ 3]==0 )&&(LFSensor[4]==0 )) 错误 =0;否则 if((LFSensor[0]==0 )&&(LFSensor[1]==1 )&&(LFSensor[2]==1 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) 错误 =- 1; else if((LFSensor[0]==0 )&&(LFSensor[1]==1 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =-2;else if((LFSensor[0]==1 )&&(LFSensor[1]==1 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =-3;else if((LFSensor) [0]==1 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) 错误 =-4; 

第七步：控制方向（比例控制-P）

完美的！此时，我们的机器人已组装完毕并可以运行。您应该对电机进行一些基本测试，读取传感器的输出并通过一条线对其进行测试。缺少的是真正的“大脑”，是“人工智能”的第一步。我们将得到这个，实现一个控制逻辑，以保证机器人将保持跟随线。

简单的比例控制：

假设机器人正在一条线上运行，传感器阵列输出为："0 0 1 0 0 " .对应的错误为“0”。在这种情况下，两个电机都应该以恒定速度向前运行。

例如：

定义变量：iniMotorSpeed =250 ;意味着左舵机将接收 1,250us 和右舵机 1,750us 的脉冲。使用这些参数，机器人将以半速向前移动。请记住，右舵机前进速度的脉冲长度范围为 1,500us（停止）到 2,000us（全速），左舵机的脉冲长度范围为 1,500us（停止）到 1,000us（全速）。

rightServo.writeMicroseconds(1500 + iniMotorPower);leftServo.writeMicroseconds(1500 - iniMotorPower); 

假设现在机器人向左行驶（就像“线路向右移动”）并覆盖传感器 3。阵列输出将为："0 0 1 1 0 " 和 error =1 .在这种情况下，您需要将机器人向右转动。为此，您必须降低右舵机的速度，这意味着减少脉冲长度。此外，左舵机的速度必须增加，这意味着减少左舵机脉冲的长度。为此，我们需要更改电机控制功能：

rightServo.writeMicroseconds(1500 + iniMotorPower - 错误); ==> 正错误：降低速度leftServo.writeMicroseconds(1500 - iniMotorPower - 错误); ==> 正错误：增加速度 

上述逻辑是正确的，但很容易理解，在脉冲长度上加减“1”微秒不会在现实时间产生所需的校正。直觉上，要加减的数字应该更大一些，例如 50、100 等。要得到这个，“误差”必须乘以一个常数（我们称之为“K”）。一旦这个常数的影响与误差成正比，我们将其命名为“比例常数：Kp” .

运动功能将是：

int Kp =50;rightServo.writeMicroseconds(1500 + iniMotorPower - Kp*error);leftServo.writeMicroseconds(1500 - iniMotorPower - Kp*error); 

我们可以恢复电机将发生的情况，如下所示：

• 传感器阵列：0 0 1 0 0 ==> 错误 =0 ==> 右伺服脉冲长度 =1,750us ==> 左伺服脉冲长度 =1,250us（两个电机速度相同）

• 传感器阵列：0 0 1 1 0 ==> 错误 =1 ==> 右伺服脉冲长度 =1,700us（较慢）==> 左伺服脉冲长度 =1,200us（较快）

如果情况相反，机器人向右行驶，则误差为“负”，舵机速度应改变：

• 传感器阵列：0 0 1 0 0 ==> 错误 =0 ==> 右伺服脉冲长度 =1,750us ==> 左伺服脉冲长度 =1,250us（两个电机速度相同）

• 传感器阵列：0 1 1 0 0 ==> 错误 =-1 ==> 右伺服脉冲长度 =1,800us（更快）==> 左伺服脉冲长度 =1,300us（更慢）

在这一点上，很明显，机器人被驱动到一侧，误差越大，返回中心的速度越快。机器人对误差的反应速度将与其成正比。这称为“比例控制” ，即更复杂的控制网络 PDI（比例、微分、积分）的“P”组件。

步骤 8：PID 控制（可选）

如果你想跳到这部分，也可以。你可以继续上一步解释的比例控制，或者烧点脑子在你的机器人中实现一个更复杂的控制系统，这是你的选择。

如果你下定决心，那就走吧！

PID（比例、微分和积分）是最常见的控制方案之一。大多数工业控制回路使用某种形式的 PID 控制。有许多方法可以调整 PID 回路，包括本示例中使用的手动技术。

将 PID 视为一个简单的弹簧。弹簧具有原始长度，当它受到膨胀或收缩的干扰时，往往会在尽可能短的时间内恢复其原始长度。类似地，系统中的 PID 算法具有要控制的特定物理量的设定值，称为‘设定点 ’，当由于某种原因更改时，系统会控制其中的其他必要功能，以在尽可能短的时间内恢复到原始设置点。 PID 控制器用于需要控制物理量并使其等于指定值的任何地方。例如，巡航控制器 汽车、机器人、温度调节器、电压调节器等

PID 如何工作？

系统计算‘error ’，或‘偏差 通过使用传感器测量该物理量的当前值，从设定点计算物理量的值。为了回到设定点，这个‘error ’应该最小化，理想情况下应该为零。此外，这个过程应该尽快发生。理想情况下，系统对其设定点变化的响应应为零滞后。

可以在许多书籍和网站中找到更多信息，包括此处。

实现PID

i) 错误项 (e)：

这等于设定点与被控制量的当前值之间的差值。错误 =set_point – current_value （在我们的例子中是从机器人在直线上的位置得到的误差变量

ii) 比例项 (P)：

此项与误差成正比。

P =错误

该值负责物理量达到设定点所需的变化幅度。比例项决定了控制回路上升时间或达到设定点的速度。

iii) 积分项 (I)：

这一项是之前所有误差值的总和。

I =I + 错误

该值负责系统对设定点变化的快速响应。积分项用于消除比例项所需的稳态误差。通常，小型机器人不使用积分项，因为我们不关心稳态误差，它会使“loop tuning ”.

iv) 微分或派生项 (D)：

这一项是设定点的瞬时误差与前一时刻的误差之差。

D =错误 - 前一个错误

该值负责在接近设定点时减慢物理量的变化率。导数项用于减少过冲或系统应该“overcorrect ”.

方程：

PID 值 =(Kp*P) + (Ki*I) + (Kd*D)

地点：

Kp 是用于改变变化幅度的常数 需要达到设定点。 Ki 是用于改变变化率的常数 应带入物理量以达到设定点。 Kd 是用于改变稳定性的常数 系统的。

一种调整循环的方法可以是试错暂定方法 :

将 Kd 变量设置为 0 并首先单独调整 Kp 项。 Kp 为 25 在我们的案例中，这是一个很好的起点。在最后一步，我们使用了 50 的 Kp，它与我的机器人配合得很好。一旦机器人做出合理的响应，调整控制回路的微分部分 (Kd ）。首先将 Kp 和 Kd 值分别设置为 Kp 值的 1/2。例如，如果机器人响应合理，Kp =50，则设置 Kp =25 和 Kd =25 以启动。增加 Kd（微分）增益以减少超调，如果机器人变得不稳定，则减少它。

• 如果机器人反应太慢，请增加该值。

• 如果机器人的反应似乎很快变得不稳定，请减小该值。

要考虑的循环的另一个组成部分是实际的采样/循环率 .加快或减慢此参数可以显着提高机器人的性能。这是由 delay 设置的 您在代码中的语句。获得最佳结果是一种尝试错误的暂定方法。

基于上述方法，实现了以下功能：

void calculatePID(){ P =error; I =I + 错误； D =错误前一个错误； PID值=（Kp*P）+（Ki*I）+（Kd*D）；以前的错误 =错误；} 

最后一步使用的简单 Kp 常数将被替换为这个更完整的 PIDvalue :

void motorPIDcontrol(){ int leftMotorSpeed =1500 - iniMotorPower - PIDvalue; int rightMotorSpeed =1500 + iniMotorPower - PID 值； leftServo.writeMicroseconds(leftMotorSpeed); rightServo.writeMicroseconds(rightMotorSpeed);} 

但请注意，如果您有 Kd 和 Ki =0 , PID 值 只是最后一步使用的Kp*error。

第 9 步：最终代码

在这一步，机器人可以遵循一个恒定的循环，并且会在不停止的情况下进行。循环程序将是：

void loop(){ readLFSsensors(); // 读取传感器，在传感器阵列中存储值并计算“错误”calculatePID(); motorPIDcontrol();} 

但是为了更完整和真实的操作，重要的是添加至少几个基本的“commands " done "用线 ”。比如我们引入一个新的变量：“mode "。我们将为这个变量定义 3 个状态：

模式：

• #define STOPPED 0

• #define FOLLOWING_LINE 1

• #define NO_LINE 2

如果所有传感器都发现一条黑线，则传感器阵列输出将是：1 1 1 1 1。在这种情况下，我们可以将模式定义为“STOPPED”，机器人应执行“句号” ”.

if((LFSensor[0]==1 )&&(LFSensor[1]==1 )&&(LFSensor[2]==1 )&&(LFSensor[3]==1 )&&(LFSensor [4]==1 )) { mode =STOPPED;} 

Follower Line Robots 的其他常见情况是它发现“no line "，或者 Sensor Array 输出为：0 0 0 0 0。在这种情况下，我们可以将其编程为向后转 180o 或以小角度转，直到找到一条线并恢复正常的 Line Follow 条件。

else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&( LFSensor[4]==0 )) { mode =NO_LINE;} 

完整的loop() 将是：

void loop() { readLFSsensors();开关（模式）{ case STOPPED:motorStop();休息; case NO_LINE:motorStop();电机转向（左，180）；休息;案例 FOLLOWING_LINE：calculatePID();电机PID控制（）；休息; }} 

真正的最终代码会集成一些额外的逻辑以及一些必须初始化的变量等。在上面的解释中，为了简单起见，我把它们省略了，但看最终代码应该很清楚。

以下是最终的 Arduino 代码：

第 10 步：使用 Android 应用程序调整 PID 控制

In the previous code, you can find at "robotDefines.h " tab the following definitions for the constants to be used with the PID control:

float Kp=50;float Ki=0;float Kd=0; 

As explained at previous step, the best way to define the correct constant to be used with a PID controller is using the "Try-error" methodology. The bad side of that is that you must re-compile the program each time that you must change it. One way to speed-up the process is to use the Android App to send the constants at the "Set-Up Phase" .

I developed an Android App exclusively for that. In short there are the traditional manual commands:

• FW, BW, Left, Right and Stop where the app will send respectively to the BT module:'f', 'b', 'l', 'r' and 's'.

Also 3 sliders were included, one for each PID constants:

• Kd:"d/XXX" where "XXX" it is a number from 0 to 100.

• Kp:"p/XXX"

• Ki:"i/XXX"

An extra button was included that will work exactly as the button connected on Arduino Pin9. You can use one or the other, it does not matter.

Below you can find the .aia file that can be modified at MIT AppInventor and the .apk file to be installed directly in your Android device.

Step 11:Changing the Code for PID remote tuning

During the Setup, we will introduce a loop where you can send the PID parameters to the Robot before you put him over the line:

 while (digitalRead(buttonPin) &&!mode) { checkBTcmd(); // verify if a comand is received from BT remote control manualCmd (); command =""; } checkPIDvalues(); mode =STOPPED; 

The manual command function will be:

void manualCmd(){ switch (command[0]) { case 'g':mode =FOLLOWING_LINE;休息; case 's':motorStop(); //turn off both motors break; case 'f':motorForward();休息; case 'r':motorTurn(RIGHT, 30); motorStop();休息; case 'l':motorTurn(LEFT, 30); motorStop();休息; case 'b':motorBackward();休息; case 'p':Kp =command[2];休息; case 'i':Ki =command[2];休息; case 'd':Kd =command[2];休息; }} 

In the video, you can see some tests using the Android App. Below the final code including the PID setup via Android:

Step 12:Conclusion

This is the first part of a more complex project, exploring the potentiality of a line follower robot. In the next part, I will develop a Maze solve robot, based on this this project here. Hopefully I can contribute for others to learn more about electronics, robot, Arduino, etc.

The update files for this project can be found at GITHUB. For more tutorials, please visit my Blog:MJRoBot.org

Greetings from the south of the world!

Thanks

Marcelo

代码

• 代码片段 #1
• 代码片段#2
• 代码片段 #3
• Code snippet #5
• Code snippet #6
• Code snippet #10
• Code snippet #11
• Code snippet #12
• Code snippet #15
• Code snippet #17
• Code snippet #18

代码片段 #1纯文本

#include  // Servo library Servo leftServo;Servo rightServo;Void setup(){ leftServo.attach(5); rightServo.attach(3); leftServo.writeMicroseconds(1500); rightServo.writeMicroseconds(1500);}void loop(){}

代码片段#2纯文本

while(digitalRead(buttonPin)) { }motorTurn (LEFT, 500);motorTurn (RIGHT, 500);

代码片段 #3纯文本

const int lineFollowSensor0 =12;const int lineFollowSensor1 =18;const int lineFollowSensor2 =17;const int lineFollowSensor3 =16;const int lineFollowSensor4 =19;

Code snippet #5Plain text

LFSensor[0] =digitalRead(lineFollowSensor0);LFSensor[1] =digitalRead(lineFollowSensor1);LFSensor[2] =digitalRead(lineFollowSensor2);LFSensor[3] =digitalRead(lineFollowSensor3);LFSensor[4] =digitalRead(lineFollowSensor4);

Code snippet #6Plain text

if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==1 )) error =4;else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==1 )&&(LFSensor[4]==1 )) error =3; else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==1 )&&(LFSensor[4]==0 )) error =2;else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==1 )&&(LFSensor[3]==1 )&&(LFSensor[4]==0 )) error =1;else if((LFSensor[0]==0 )&&(LFSensor[1]==0 )&&(LFSensor[2]==1 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =0;else if((LFSensor[0]==0 )&&(LFSensor[1]==1 )&&(LFSensor[2]==1 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =- 1;else if((LFSensor[0]==0 )&&(LFSensor[1]==1 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =-2;else if((LFSensor[0]==1 )&&(LFSensor[1]==1 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =-3;else if((LFSensor[0]==1 )&&(LFSensor[1]==0 )&&(LFSensor[2]==0 )&&(LFSensor[3]==0 )&&(LFSensor[4]==0 )) error =-4;

Code snippet #10Plain text

void calculatePID(){ P =error; I =I + error; D =error-previousError; PIDvalue =(Kp*P) + (Ki*I) + (Kd*D); previousError =error;}

Code snippet #11Plain text

void motorPIDcontrol(){ int leftMotorSpeed =1500 - iniMotorPower - PIDvalue; int rightMotorSpeed =1500 + iniMotorPower - PIDvalue; leftServo.writeMicroseconds(leftMotorSpeed); rightServo.writeMicroseconds(rightMotorSpeed);}

Code snippet #12Plain text

void loop (){ readLFSsensors(); // read sensors, storage values at Sensor Array and calculate "error" calculatePID(); motorPIDcontrol();}

Code snippet #15Plain text

void loop() { readLFSsensors(); switch (mode) { case STOPPED:motorStop();休息; case NO_LINE:motorStop(); motorTurn(LEFT, 180);休息; case FOLLOWING_LINE:calculatePID(); motorPIDcontrol();休息; }}

Code snippet #17Plain text

 while (digitalRead(buttonPin) &&!mode) { checkBTcmd(); // verify if a comand is received from BT remote control manualCmd (); command =""; } checkPIDvalues(); mode =STOPPED;

Code snippet #18Plain text

void manualCmd(){ switch (command[0]) { case 'g':mode =FOLLOWING_LINE;休息; case 's':motorStop(); //turn off both motors break; case 'f':motorForward();休息; case 'r':motorTurn(RIGHT, 30); motorStop();休息; case 'l':motorTurn(LEFT, 30); motorStop();休息; case 'b':motorBackward();休息; case 'p':Kp =command[2];休息; case 'i':Ki =command[2];休息; case 'd':Kd =command[2];休息; }}

Github

示意图