Java-机器人编程入门手册-全-
Java 机器人编程入门手册（全）
原文：Beginning Robotics Programming in Java with LEGO Mindstorms
一、乐高 MindStorm 和 leJOS 简介
Electronic supplementary material The online version of this chapter (doi:10.1007/978-1-4842-2005-4_1) contains supplementary material, which is available to authorized users.
本章提供了如何使用乐高 MindStorm EV3 公司建立 Java 机器人编程环境的分步指南，包括乐高 MindStorm EV3 公司的基本概述和莱霍斯·EV3 公司的介绍。本章包括如何在你的计算机上安装莱霍斯 EV3 开发系统，如何在乐高 EV3 积木上安装莱霍斯 EV3 固件，以及如何安装和应用 Eclipse IDE 的莱霍斯 EV3 插件。最后，您将创建第一个名为 HelloWorld 的 Java 机器人程序，通过 USB 电缆将 HelloWorld 程序从您的计算机上传到乐高 EV3 积木中，并在乐高 Mindstorms EV3 上执行该程序。
乐高 MindStorm 简介
乐高 MindStorm 是一款来自乐高的教育产品，旨在帮助您轻松构建机器人。该产品系列已经发展了几十年，乐高 MindStorm EV3 是第三代。图 1-1 展示了一个典型的乐高 MindStorm EV3 机器人套装，其中 EV3 砖块是乐高 MindStorm EV3 的大脑。这是一个智能的可编程设备，让乐高机器人执行各种智能操作。
图 1-1。
Lego Mindstorms EV3
如图 1-1 所示，乐高 Mindstorms EV3 的典型组件包括电机端口、传感器端口、PC 连接端口、扬声器和 EV3 按钮。关于乐高 MindStorm EV3 组成部分的更多细节可以在: http://www.lego.com/en-us/mindstorms/downloads 找到。
乐高 MindStorm EV3 中的电机端口有四个用于连接电机的输出端口:端口 A、B、C 和 d
传感器端口有四个用于连接传感器的输入端口:端口 1、2、3 和 4。
迷你 USB PC 连接端口用于将 USB 电缆连接到您的本地计算机，并将程序下载到 EV3 砖块(或将数据从机器人上传到您的本地计算机)。您还可以使用无线蓝牙连接上传和下载程序。
乐高 MindStorm EV3 中包含的扬声器使程序具有真实的声音成为可能，你可以在运行程序时听到它们。
您可以在 EV3 砖块的中心应用一个深灰色按钮来打开电源、输入命令或运行程序。砖块的左上角还有一个浅灰色的按钮，用于反转动作、中止程序和关闭 EV3。EV3 砖块上的其他四个浅灰色按钮用于在 EV3 菜单中向左、向右、向上和向下移动。
乐高 MindStorm EV3 的典型技术规格如下。关于乐高 EV3 规格的更多细节可以在 http://www.lego.com/en-us/mindstorms/downloads 找到。
- 单个主处理器控制机器人:
- 运行频率为 300 MHz 的 32 位 ARM9 处理器
- 能够访问 64 MB 内存
- 使用 16 MB 闪存
- 操作系统是基于 Linux 的
- 使用 6 节 AA 电池或稍大的可充电电池组运行:
- 虽然 6 节 AA 电池理论上相当于 9 伏，但你更可能体验到大约 7-8 伏，这取决于电池中的电荷
- 包含四个电机/伺服端口:
- 三个马达(两个大型马达和一个中型马达)配有每个乐高 MindStorm EV3 套件
- 包含四个传感器端口:
- 配有各种传感器:
- -触摸
- -颜色
- 超声波
- 陀螺仪
- 配有各种传感器:
- 交流:
- 自带蓝牙
- 一个程序可以加载到 EV3 砖使用蓝牙以较慢的速度或使用 USB 电缆以更快的速度
- 可以被编程以允许在程序执行时在两个(或多个)EV3 砖块之间进行通信
- 当程序正在执行时，第三方软件可用于 PC 和 EV3 砖之间的通信
- 包括一个内置的 178 × 128 像素 LCD 图形显示屏
leJOS 简介
“leJOS”的意思是 Lego for Java 操作系统，这是一种开源语言，用于使用 Java 技术为 Lego Mindstorms 产品开发软件。leJOS 项目支持乐高 MindStorm EV3，以及以前的版本，包括乐高 MindStorm NXT 和乐高 MindStorm RCX。leJOS 项目为 Lego Mindstorms 提供了以下解决方案:
乐高 Mindstorms EV3
- EV3 砖的 JVM
- EV3 砖的 leJOS API
- leJOS PC 通信
- 远工具
乐高 Mindstorms NXT
- 用于 NXT 块的 JVM
- 用于 NXT 砖的 leJOS API
- leJOS PC 通信
- leJOS JavaME 通信公司
- 远工具
乐高 Mindstorms RCX
- RCX 砖的 JVM
- RCX 砖的 leJOS API
- leJOS PC 通信
- 远工具
在这本书里，我们关注的是最新的乐高 MindStorm 产品:也就是乐高 MindStorm EV3。表 1-1 显示了 leJOS 为 EV3 砖提供的典型官方包装。这些软件包允许你管理 EV3 砖，传感器，执行器，以及其他一些 EV3 硬件。
表 1-1。
EV3 brick packages
| 包裹 | 描述 | | --- | --- | | 远点，硬件 | 支持 EV3 硬件 | | 远点，硬件，ev3 | 要访问 EV3 硬件 | | 远点，硬件，lcd | 要访问 EV3 液晶显示器 | | 很远，硬件，引擎 | 去 EV3 汽车公司 | | 远点，硬件，端口 | 进入 EV3 港口 | | 远点，硬件，传感器 | 要访问 EV3 上支持的所有传感器 | | 远点，硬件，视频 | 要访问视频设备 |表 1-2 列出了为一些机器人问题提供支持的包，比如定位和导航。
。
Robotics/AI packages
| 包裹 | 描述 | | --- | --- | | lejos .机器人.本地化 | 本地化支持 | | lejos .机器人学.制图 | 支持地图 | | lejos .机器人.导航 | 导航类 | | lejos.robotics .对象检测 | 对象检测类 | | 机器人学包容 | 支持包容架构 |所有 leJOS 版本都有 Javadoc 格式的包文档。有关 EV3 莱霍斯酒店提供的套餐详情，请访问: http://www.lejos.org/ev3/docs/ 。
图 1-2 显示了莱霍斯 EV3 开发文件。
图 1-2。
leJOS development documents
JDK 装置
leJOS 项目基于 Java 技术，所以您需要在本地计算机上安装 Java 开发工具包(JDK)当前版本。JDK 发布可以在: http://www.oracle.com/technetwork/java/index.html 找到。
Java 运行时环境(JRE)是不够的，因为它不允许您编译 Java 程序。莱霍斯·EV3 只支持 32 位版本的 JDK 和 JRE，所以即使你有 64 位系统，你也应该选择 32 位版本的 JDK。此外，莱霍斯·EV3 已经在 1.7 版上进行了测试，因此本书推荐使用 Java 7。作为一个例子，下面的步骤向您展示了如何使用名为jdk-7u45-windows-i586.exe的 Java JDK 安装程序来安装 JDK。
Installing The Jdk
-
Double-click the file
jdk-7u45-windows-i586.exe, and you will see the screen shown in Figure 1-3. Then, click the Next button.
图 1-3。
Step 1 of the JDK Installation
-
Install the JAVA JDK to the path
C:\Program Files (x86)\Java\jdk1.7.0_45, choose all components, and click the Next button, as illustrated in Figure 1-4. It will then install JDK components that you chose.
图 1-4。
Step 2 of the JDK installation
-
Click the Close button, as shown in Figure 1-5. The JAVA JDK is then successfully installed on your computer at:
C:\Program Files (x86)\Java\jdk1.7.0_45.
图 1-5。
Step 3 of the JDK installation
-
一旦你在电脑上安装了 J2SE 软件开发工具包，就有必要检查你是否能编译和执行任何 java 程序。
测试 JDK 安装
在您的计算机上打开一个 Shell 控制台，并键入命令Java:
Java:用于执行 Java 程序的 Java 命令Javac:用于编译 Java 程序的 Java 命令
执行第一个测试的原因是因为您需要检查您的操作系统是否识别用于执行 Java 程序的命令java。如果 shell 控制台返回使用该命令的选项，如图 1-6 所示，则测试成功。
图 1-6。
Test running the java command
第二个测试是必要的，以了解您的操作系统是否识别命令javac，该命令用于编译您的程序。在键盘上键入javac并检查消息。如果您的系统不能识别该命令，那么您必须更新计算机系统中的环境变量。右键单击“我的电脑”图标，然后选择属性。单击“高级选项”选项卡。当您单击此命令时，您将看到一个新窗口，您可以在其中更新变量路径。该路径用于直接从 Shell 键盘执行命令。
在选项卡高级选项选项卡上，点击环境变量按钮，如图 1-7 所示。path变量位于系统变量区。找到变量path，点击更新按钮。路径变量可能有许多语句，因为它们被许多应用使用。
图 1-7。
A screenshot of setting up environment variables
要更新path变量，在你的计算机上找到 J2SE SDK 所在的路径。在这种情况下，路径是:
C:\Program Files (x86)\Java\jdk1.7.0_45\bin
知道路径后，将其添加到系统变量 path 内容的末尾。
您还需要创建一个名为JAVA_HOME的新系统变量，并将其值设置为C:\Program Files (x86)\Java\jdk1.7.0_45。一旦你做了更改，重新启动 DoS 命令窗口，并再次检查命令javac。
如果您看到命令javac的选项，如图 1-8 所示，那么测试是成功的——恭喜！您已经完成了 JDK 的安装。现在您可以使用您的计算机来开发 java 程序，并且您已经安装和配置了基本的 Java 工具。
图 1-8。
Test running the javac command
在乐高 EV3 上安装 leJOS 及其固件
leJOS 可以安装在主流的操作系统中，比如 Windows、Linux 和 Mac OS。既然你已经安装并运行了 Java，那么是时候在你的电脑上安装 leJOS 系统，并在乐高 Mindstorms EV3 上安装它的固件了。为此，您需要一张最大容量为 32GB 的空白 SD 卡。此外，SD 卡需要使用 FAT32 分区进行格式化。安装 leJOS 最简单的方法是在: http://www.lejos.org 下载安装程序。
Installing Lejos
-
访问
https://sourceforge.net/projects/ev3.lejos.p/files/0.9.1-beta/，然后选择Download leJOS_EV3_0.9.1-beta_win32_setup.exe (41.8MB)。然后你可以下载并保存leJOS_EV3_0.9.1-beta_win32_setup.exe到你的电脑上。 -
Double-click
leJOS_EV3_0.9.1-beta_win32_setup.exe, and you will see a leJOS EV3 Setup Wizard, as shown in Figure 1-9.
图 1-9。
leJOS setup wizard
-
Click the Next button, and you will see the screen shown in Figure 1-10.
图 1-10。
Choosing the right JDK for use with leJOS EV3
-
Choose the 32-bit JDK that you installed, for example,
jdk1.7.0_45, and then click the Next button. You will see the screen shown in Figure 1-11.
图 1-11。
Choosing a folder to install leJOS EV3 program
-
Make sure that you have installed the JDK and set the path and
JAVA_HOMEto the installation directory of your JDK. You can click browse to select the path to which you want to install. In this example, I chose the destination folderC:\Program Files (x86)\leJOS EV3. After you set up the installation folder, click Next button and you will see the screen shown in Figure 1-12.
图 1-12。
Choosing ALL components of leJOS
-
Check to choose all of the components you wish to install, and then click Next button. You will see the screen shown in Figure 1-13.
图 1-13。
Selecting folders where to install the sample leJOS projects
-
Choose to install the Sample Projects and the Development Kit into root folder C, click Next, and then you can use the default setting to create a Start Menu folder called
leJOS EV3. After that, click Next and you will see the screen shown in Figure 1-14.
图 1-14。
General settings for the leJOS installation
-
Double-check to see if all of the settings are OK, and then click Install. You will see the installation progress bar, and eventually you will see the screen shown in Figure 1-15.
图 1-15。
Finishing the leJOS installation
-
Make sure that you have your SD card ready, and then click Finish. After that, a EV3SDCard utility program will start, as shown in Figure 1-16.
图 1-16。
EV3 SD Card creator
-
Choose the right SD Card drive, click the link to Download the EV3 Oracle JRE, and select the corresponding
.gzfile. Then click the Create button and you will see that the EV3 firmware is burned into the SD card, as shown in Figure 1-17.
图 1-17。
Installing the leJOS firmware into the SD card
创建 SD 卡后，将它插入 EV3 砖，按下中间的深灰色按钮启动 EV3，并在 EV3 砖上完成 leJOS 固件安装。leJOS EV3 的标志将显示在砖块的 LCD 上。安装完成后，将显示一个 leJOS EV3 菜单，默认 IP 地址显示在顶部(10.0.1.1)。至此，您已经准备好进行下一步，将用于开发 leJOS 程序的 Eclipse 插件安装到 EV3 砖中。
莱霍斯·EV3 的 Eclipse IDE 和 Eclipse 插件
当然，仅仅使用文本编辑器和命令行也可以进行 Java 编程。然而，对程序员来说，点击按钮让事情发生要比输入命令和可选参数容易得多。一般来说，标准的文本编辑器并不包含很多在编辑代码时可以帮助你的特性，而且当你拼错了一个类名或者遗漏了一个括号时，它们也不会告诉你。集成开发环境(IDE)是一种工具，允许您使用简单的按钮输入、编译和上传代码到 EV3，它还通过对代码进行颜色编码来监控代码语法，以便您可以轻松识别关键字和变量。最好的开源 ide 之一是 IBM 的 Eclipse，它是免费的，功能强大，易于使用。这一节将向您展示如何在 leJOS EV3 上为 Java 编程设置 Eclipse IDE。
第一步，在: http://www.eclipse.org/downloads/packages/eclipse-ide-java-developers/indigosr1 下载 Eclipse，搜索面向 Java 开发者的 Eclipse IDE。请注意，即使您使用的是 64 位计算机，也需要下载 32 位版本的 Eclipse IDE，因为莱霍斯·EV3 插件不能与 64 位版本的 Eclipse 一起工作。下载的是一个.zip文件，您必须将文件解压缩到一个目录中。这将是 Eclipse 程序的永久位置。要运行 Eclipse，只需双击 Eclipse 目录中的可执行文件。类似地，为了删除 Eclipse，您只需从您的计算机上删除 Eclipse 目录。
第一次运行 Eclipse 时，它会询问您工作区的位置。在本实验中，您可以选择工作区作为用于安装 leJOS 的文件夹，在本例中为C:\leJOSEV3Proj，如图 1-18 所示。
图 1-18。
Setting up the workspace for Eclipse
如果有软件补丁或新功能，您可以通过点击Window -> Preferences，双击列表中的Install/Update，突出显示自动更新，将 Eclipse 设置为自动搜索更新。如图 1-19 所示，勾选自动查找新更新并通知我，然后点击确定。之后，Eclipse 及其插件将自动更新。
图 1-19。
Setting up automatic updates in Eclipse Installing The Eclipse Ide And Eclipse Plu-Gin
既然已经安装了 Eclipse，那么是时候安装莱乔斯 have 插件了。请遵循以下步骤:
-
In Eclipse, select
Help > Install New Software. You will see a dialog requesting that you input a URL, as shown in Figure 1-20.
图 1-20。
Step 1 of installing the leJOS EV3 plug-in
-
Click Add, and you will see another dialog box, as shown in Figure 1-21. Enter the name leJOS EV3, and for the location enter this:
http://lejos.sourceforge.net/tools/eclipse/plugin/ev3.
图 1-21。
Step 2 of installing the leJOS EV3 plug-in
-
Click OK. You should see a new item in the main dialog box, as shown in Figure 1-22. Place a check mark in the box next to the new item, and click the Next button.
图 1-22。
Step 3 of installing the leJOS EV3 plug-in
-
阅读并接受许可协议，然后单击“下一步”按钮。插件将自动安装。
-
完成后，会要求您重启 Eclipse。一旦它重新启动，您将在 Eclipse 中看到一些微妙的变化。该插件将在 Eclipse 中的许多地方添加新的 leJOS 菜单项。
-
Eclipse will automatically look for the
EV3_HOMEenvironment variable to locate leJOS EV3. Check to make sure that the preferences are what you like. SelectWindows -> Preferencesand then leJOS EV3 from the list. If the leJOS EV3 directory is not correct, either type in the location or browse to it. Make sure that you browse to the main directory and not one of its subdirectories. After that, you need to double-check if they are the same as the items illustrated in Figure 1-23.
图 1-23。
Preferences for the leJOS EV3 plug-in
Creating And Uploading A Program: Helloworld
现在您需要创建一个输入代码的地方。Eclipse 将单个 Java 项目保存在自己的项目目录中。例如，如果您创建一个处理映射的大型多类项目，您将在它自己的目录中创建您自己的项目来存储类和数据文件。
在本节中，您将创建一个用于存储代码的项目。
-
选择
File > New > Project。 -
In the next window, double-click leJOS EV3 to expand the folder options. You want to create a leJOS EV3 project, so select leJOS EV3 project and click the Next button, as shown in Figure 1-24.
图 1-24。
A New leJOS project
-
For the project name , enter
testand then click Finish, as shown in Figure 1-25.
图 1-25。
Create a new leJOS EV3 project
-
In order to add a new class file, select
File > New > Class. EnterHelloWorldin the name field, as shown in Figure 1-26. Eclipse will also offer other options, such as automatically adding amain()method. Check this if you want Eclipse to do some of the typing for you.
图 1-26。
Add a class in the new leJOS EV3 project
-
完成后点击
Finish按钮。您应该会看到一个包含一些起始代码的新类文件。在文件中输入随后的HelloWorld代码。//************************************************************ //Wei Lu Java Robotics Programming with Lego EV3 Hello World.java //An example to display HelloWorld on the LCD screen of EV3 brick //************************************************************ // import EV3 hardware packages for EV brick finding, // activating keys and LCD import lejos.hardware.ev3.EV3; import lejos.hardware.BrickFinder; import lejos.hardware.Keys; import lejos.hardware.lcd.TextLCD; public class HelloWorld { public static void main(String[] args) { // get EV3 brick EV3 ev3brick = (EV3) BrickFinder.getLocal(); // instantized LCD class for displaying and Keys // class for buttons Keys buttons = ev3brick.getKeys(); TextLCD lcddisplay = ev3brick.getTextLCD(); // drawing text on the LCD screen based on // coordinates lcddisplay.drawString("HelloWorld", 2, 4); // exit program after any button pressed buttons.waitForAnyPress(); } } -
Click the Save button, turn on your EV3 brick, and then click the green Run button in the Eclipse toolbar. A pop-up window will appear the first time that you click the Run button for a class file. Select the leJOS EV3 program, as shown in Figure 1-27, and click OK. The program will begin uploading. You need to make sure that the EV3 brick is connected to your computer through the mini-USB port .
图 1-27。
Running a leJOS EV3 program
当程序上传并自动运行时，您的 EV3 将在屏幕中央显示HelloWorld，假设您使用的是默认设置。如果您不想让程序在每次上传时自动运行，请选择Windows > Preferences > leJOS EV3并取消选中上传后运行程序。
摘要
本章让你开始在你的本地计算机上安装 JDK 和莱霍斯系统，并使用外部 SD 卡在乐高 MindStorm EV3 积木上刻录莱霍斯 EV3 固件。您还使用 leJOS 和您的机器人运行了第一个名为HelloWorld的 Java 机器人程序。leJOS 可以安装在主流的操作系统中，比如 Windows、Linux 和 Mac OS。特别是，您在本章中学到了以下内容:
- 如何安装莱乔斯 EV3 软件到您的 Windows 操作系统与莱乔斯安装程序。
- 如何安装莱霍斯 EV3 固件到你的乐高 EV3 砖。
- 如何为 Eclipse IDE 安装和使用 leJOS 插件。
- 如何在 Eclipse 中编写源代码，然后上传并在您的 EV3 砖上运行程序。
- 如何在本地计算机上安装和配置 JDK。
在下一章中，您将了解 EV3 大型电机及其在莱霍斯 EV3 提供的相应电机类别。然后，在此基础上，你将学习如何使用电机控制基本运动，如何中断旋转，如何调节电机速度，以及如何跟踪直线。
二、电机传感器简介
本章介绍了 EV3 大型伺服电机及其在 EV3 莱霍斯提供的相应电机类别。特别是，本章包括六个示例 Java 项目，涵盖以下主题:
- 使用电机控制基本运动
- 使用转速表进行惯性测试
- 控制电机的精确旋转
- 中断旋转
- 调节电机速度
- 描绘一条直线
Java 编程的基本概念
莱霍斯 EV3 基础设施使用 Java 编程语言为乐高 MindStorm EV3 机器人开发和实现软件系统。Java 是一种面向对象的编程语言，已经广泛应用于软件工程行业。这本书的目的不是指导你学习 Java 编程语言。相反，它假设您已经有了一些基本的 Java 编程经验。
任何 Java 程序都有一个用来管理所有操作的主要部分。在 Java 中，所有文件都是类，但是只有一个 Java 类具有方法main。在你在第一章中开发的示例程序HelloWorld.java中，公共方法main在下面的程序中说明:
// import EV3 hardware packages for EV brick finding,
// activating keys and LCD
import lejos.hardware.ev3.EV3;
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.lcd.TextLCD;
public class HelloWorld {
public static void main(String[] args) {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys // class for buttons
Keys buttons = ev3brick.getKeys();
TextLCD lcddisplay = ev3brick.getTextLCD();
// drawing text on the LCD screen based on
// coordinates
lcddisplay.drawString("Hello World", 2, 4);
// exit program after any button pressed
buttons.waitForAnyPress();
}
}
在上面的HelloWorld.java程序清单中，lcddisplay.drawString用于在乐高 EV3 LCD 显示屏上打印文本Hello World，类似于在 DoS 命令窗口上打印Hello World的语句System.out.println("Hello World")。
在这个例子中，HelloWorld. java是一个简单的项目，有一个唯一的类。然而，在实践中，一个 Java 项目通常是复杂的，所以通常你会有多个类，其中一个将操纵其余的类。总之，任何 Java 类都包括一个导入区域、类封装和main方法。导入区域声明您将在一个特定的类中使用什么 Java 包。例如，在HelloWorld.java程序中，您使用 EV3 键和 LCD 功能，然后需要您导入包lejos.hardware.Keys和lejos.hardware.lcd.*。在main方法中，main类需要一个main方法来执行它。在HelloWorld类的例子中，它有一个main类用来执行一些指令；即lcddisplay.drawString("Hello World", 2, 4)在 LCD 屏幕上打印第 2 行第 4 列的Hello World，按下 EV3 砖上的任意按钮后buttons.waitForAnyPress()退出程序。
电机简介
马达是乐高 EV3 套件中所有运动的来源，它们在本书的所有机器人项目中扮演着重要的角色。因此，本章将使您熟悉莱霍斯 EV3 公司提供的电机类别的操作。
使用在莱霍斯 EV3 中实现的运动算法，你可以设置 EV3 机器人精确地旋转到指定的度数，比如 240 度，而不越过这个目标度数。此外，您可以为轮式车辆设置恒定的速度，这可以通过实时调整功率水平来实现，这样当机器人向上或向下移动时，它可以保持与您建立的速度相同的速度。此外，电机类别使电机能够通过最初以低速启动并不断加速达到指定速度来加速到全速。因此，您可以命令电动机在一个方向上前进数千圈，然后让它们随时返回原点。这些功能为 EV3 机器人的创造性导航和手臂旋转打开了无限的思路。
要使用乐高 EV3 机器人套装中提供的三个马达，你必须导入lejos.hardware.motor包。该软件包带有四个字段:A、B、C 和 D，用于四个电机端口，以及许多方法。关于这样做的细节，你可以在 http://www.lejos.org/ev3/docs/ 查看 EV3 的 Java API 函数。例如，您可以使用以下语句设置连接到端口 A 的电机的速度:
EV3LargeRegulatedMotor LEFT_MOTOR =
new EV3LargeRegulatedMotor(MotorPort.A);
LEFT_MOTOR.setSpeed(720);
这段代码将设置LEFT_MOTOR的速度，使其每秒滚动 720 度(即每秒旋转两次)。如果你想让这个马达继续前进，你可以这样说:
LEFT_MOTOR.forward();
接下来，提供六个示例 Java 项目来介绍用于控制马达的马达类。您需要编译、上传并运行所有六个示例程序来完成本章中的所有任务。
介绍电机类
电机类提供对 EV3 大型伺服电机的访问。当控制 EV3 的运动时，电机必须连接到四个 EV3 电机端口中的一个。该类为每个端口提供了一个实例，即 MotorPort。答:汽车港。b，汽车港。c 和 MotorPort.D。
这四个对象中的每一个都是类EV3LargeRegulatedMotor的一个实例，它提供了控制电机的方法。在本节中，给你一套六个程序，通过使用它们，你可以进行实验，以了解 EV3 大型电机如何运行。这些程序足够简单，所以编写它们不需要太多 Java 编程经验。尽管如此，它们仍然可以让你对编程和控制电机运动有一个基本的了解。
使用电机控制基本运动
这个程序使用控制运动的基本运动方法。本程序中使用的方法包括表 2-1 中所示的方法:
表 2-1。
Basic Motor Methods
| 班级 | 方法 | 功能 | | --- | --- | --- | | ev3 大型调节电机 | `forward()` | 电机正转。 | | | `backward()` | 电机反向旋转。 | | | `stop()` | 电机快速停止。 | | 键 | `waitForAnyPress` | 等到任何键被按下。 | | 液晶显示 | `drawString(String str,` `int x, int y)` | 在 lcd 的捐赠行 x 和 y 列打印一个文本字符串。 |该程序应该执行以下操作:
- 正向运行电机 A 和 C。
- 在顶行向前显示。
- 等到按钮被按下。
- 反向运行电机 A 和 C。
- 在下一行向后显示。
- 等到按钮被按下。
- 停止电机 A 和 c。
以下程序实现了上面定义的电机运动:
//***************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 p1.java
//Simple motor testing
//***************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
public class p1 {
public static void main(String[] args) {
EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.C);
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying
// and Keys class for buttons
Keys buttons = ev3brick.getKeys();
// block the thread until a button is pressed
buttons.waitForAnyPress();
// move robot forward and display status on LCD
// change directions when button is pressed
LEFT_MOTOR.forward();
RIGHT_MOTOR.forward();
LCD.drawString("FORWARD", 0, 0);
// block the thread until a button is pressed
buttons.waitForAnyPress();
// move robot backward and display status on LCD
LEFT_MOTOR.backward();
RIGHT_MOTOR.backward();
LCD.drawString("BACKWARD", 0, 1);
// block the thread until a button is pressed
buttons.waitForAnyPress();
// stop robot and display status on LCD
LEFT_MOTOR.stop();
RIGHT_MOTOR.stop();
LCD.drawString("STOP", 0, 2);
// exit program after any button pressed
buttons.waitForAnyPress();
}
}
使用转速表进行惯性测试
EV3 大型伺服电机有一个内置的转速表，跟踪电机轴的当前角度(以度为单位)。该程序的目的是使用转速表找出电机停止的速度。
该程序应该执行以下操作:
- 将电机速度设置为 720。
- 运行 MotorPort。一个前锋。
- 等到转速表计数达到 720。
- 停止马达。
- 在 LCD 上显示转速表读数。
- 等到电机实际停止。
- 在 LCD 上再次显示转速表读数。
- 等待按钮按下，给你时间来记录屏幕显示。
由于马达的惯性，你会发现在你调用方法stop()后，马达并没有立即停止。本程序中使用的新方法包括表 2-2 中所示的方法:
表 2-2。
Methods Used in Tachometer
| 班级 | 方法 | 功能 | | --- | --- | --- | | ev3 大型调节电机 | `getTachoCount ()` | 获取以度为单位的电机角度。 | | | `resetTachoCount ()` | 将计数器数字重置为 0。 | | | `setSpeed(int speed)` | 以每秒度数设置旋转速度。 | | | `getRotationSpeed()` | 获取电机的实际速度，单位为每秒度数。 | | | `clear()` | 清除 LCD 屏幕。 |//************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 p2.java
//Motor inertia test
//************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
public class p2 {
public static void main(String[] args) {
EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys // class for buttons
Keys buttons = ev3brick.getKeys();
// block the thread until a button is pressed
buttons.waitForAnyPress();
// set motor to move 720 degrees per second
LEFT_MOTOR.setSpeed(720);
// start forward movement
LEFT_MOTOR.forward();
// a counter to count the number of degrees
// rotated
int count = 0;
// continue moving until motor has rotated 720
// degrees
while (count < 720)
count = LEFT_MOTOR.getTachoCount();
// stop the motor
LEFT_MOTOR.stop();
// display the tachometer reading
LCD.drawString("Tacho Read: " + count, 0, 0);
// wait for motor to actually stop and display
// tacho count.
// this number will be higher than previous due // to motor inertia
while (LEFT_MOTOR.getRotationSpeed() > 0);
LCD.drawString("Tacho Read: " + LEFT_MOTOR.getTachoCount(), 0, 1);
// block the thread until a button is pressed
buttons.waitForAnyPress();
LCD.clear();
}
}
运行上述程序后，你会发现液晶屏上印着两个数字。这两个数字是不同的。第一个数字显示调用stop()方法时转速计的读数。您可能希望这个数字是 720，因为程序在调用方法stop()之前会等待转速计达到 720。然而，第二个数字高于第一个。这是因为当调用stop()方法时，由于电机的惯性，电机不会立即停止。
控制电机的精确旋转
Motor 类提供了一个一直运行的调节器线程，您可以使用它在指定的角度停止电机。在这个例子中，您将运行一个测试来评估rotate()方法的准确性。本程序中使用的方法包括表 2-3 中所示的方法:
表 2-3。
Methods Used in the Motor Class Regulator Thread
| 班级 | 方法 | 功能 | | --- | --- | --- | | ev3 大型调节电机 | `rotate(angle)` | 旋转定义角度的度数。 | | | `rotateTo(angle)` | 旋转到指定的角度。 |该程序应该执行以下操作:
- 将速度设置为 720。
- 将电机旋转一整圈。
- 在 LCD 第 0 行显示转速表读数。
- 将电机旋转至 360°角。
- 在 LCD 第 1 行显示转速表读数。
- 等待按钮按下，给你时间阅读液晶显示器。
- 清空液晶显示器。
根据电机调节器，电机通常在距指定角度不到 1 度时停止。这是通过计算制动后电机将继续运行多远来实现的。在达到指定角度之前应用制动，然后进行微调以微调电机位置，直到它足够接近。
//***************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 p3.java
//This program demonstrated motor rotation control
//***************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
public class p3 {
public static void main(String[] args) {
EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
// block the thread until a button is pressed
buttons.waitForAnyPress();
// set motor to move 720 degrees per second
LEFT_MOTOR.setSpeed(720);
// rotate the motor one full revolution
LEFT_MOTOR.rotate(360);
// display the tacho count in row 0
LCD.drawString("Tacho Read: " + LEFT_MOTOR.getTachoCount(), 0, 0);
// rotate to the angle 360
LEFT_MOTOR.rotateTo(360);
// display the tacho count in row 1
LCD.drawString("Tacho Read: " + LEFT_MOTOR.getTachoCount(), 0, 1);
// block the thread until a button is pressed
buttons.waitForAnyPress();
LCD.clear();
}
}
通过在乐高 EV3 上运行上述程序，第一个转速表读数可能是 360，第二个是 359(或 360，取决于电机)。这两个数字相差不大。程序的第一步是旋转一整圈。那么它应该将电机设置到 360°位置。由于一次旋转是 360 度，因此预计这些数字会很接近。通过程序的演示，您可以观察到移动到某个位置比调用stop()方法可以获得更好的精度。马达类预测马达应该停止的时间，并且更早地应用制动以弥补惯性的影响。
中断旋转
有时你会希望电机在达到某一特定角度之前停止或做些别的事情。在此示例程序中，如果您足够快地按下一个按钮，您将让代码检测到中断循环作业的按钮按下。在电机停止在目标角度之前，rotate()方法不会返回。但是，您在这个程序中使用的新方法可以立即返回。本程序中使用的一些新方法包括表 2-4 中所示的方法:
表 2-4。
Rotate Method Used in the Motor Class
| 班级 | 方法 | 功能 | | --- | --- | --- | | ev3 大型调节电机 | `rotate(angle, immediateReturn)` | 旋转定义角度的度数，同时如果`immediateReturn`的值为真，该方法立即返回。 | | | `rotateTo(angle, immediateReturn)` | 旋转到指定的角度，同时如果`immediateReturn`的值为真，该方法立即返回。 | | | `(boolean) isMoving()` | 如果电机一直在旋转，则返回 true。 | | | `int readButtons()` | 如果按下任何按钮，返回按钮 id 号。 |该程序应该执行以下操作:
- 开始旋转 7200 度。
- 当电机旋转时，显示第 0 行位置的转速表计数。
- 按下按钮时，停止电机。
- 电机停止后，显示第 0 行位置的转速表计数。
- 记录从转速表读取的两个数字，然后等待按钮退出程序。
当您在旋转完成前按下按钮时，电机将停止旋转。
//************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 p4.java
//interrupting motors using buttons
//************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.Sound;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
public class p4 {
public static void main(String[] args) {
EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys // class for buttons
Keys buttons = ev3brick.getKeys();
// sound two beeps before starting program
Sound.twoBeeps();
// block the thread until a button is pressed
buttons.waitForAnyPress();
// rotate 7200 degree during which the method has // returned value all the time
LEFT_MOTOR.rotate(7200, true);
// return to true if the motor is always rotating
while (LEFT_MOTOR.isMoving()) {
// display and refresh the tachometer reading all // the time
LCD.drawString("Tacho Read: " + LEFT_MOTOR.getTachoCount(), 0, 0);
// determining if there is any button
// pressed, if yes then stop the motor
if (buttons.readButtons() > 0)
LEFT_MOTOR.stop();
}
// wait until the motor fully stopped
while (LEFT_MOTOR.getRotationSpeed() > 0)
;
// display the tachometer reading after motor
// fully stopped
LCD.drawString("Tacho Read: " + LEFT_MOTOR.getTachoCount(), 0, 1);
// block the thread until a button is pressed
buttons.waitForAnyPress();
}
}
调节电机速度
电机类有一个调节器螺纹来控制电机速度。这样做的原因是，只有当两个马达以相同的速度运行时，两轮车才会直线行驶。leJOS EV3 保持每个电机旋转与系统时钟同步，因此调节器将转速表计数与速度乘以运行时间进行比较。然后，它调整功率，使这两个数字尽可能匹配。本程序中使用的一些新方法包括表 2-5 中所示的方法:
表 2-5。
Regulator Methods Used in the Motor Class
| | | 功能 | | --- | --- | --- | | 跑表 | `elapsed()` | 返回以毫秒为单位的运行时间。 | | | `reset()` | 将手表归零。 |秒表类包含在包lejos.util.Stopwatch中。该程序应该执行以下操作:
- 创建一个新的秒表。
- 启动两台电机 A 和 C，以 1 转/秒的速度运行。(一转就是 360。)
- 每隔 200 毫秒，在同一行显示两个转速表计数值。
- 重复步骤 3 四次，每次使用不同的行。
- 打印出电机转速计计数之间的最大差值。
根据你的观察，这些电机应该保持在几度之内，因为我们使用了调节的大型电机 EV3 级。
//************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 p5.java
//Motor speed control comparison
//************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.utility.Stopwatch;
public class p5 {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.C);
public static void main(String[] args) {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys // class for buttons
Keys buttons = ev3brick.getKeys();
// block the thread until a button is pressed
buttons.waitForAnyPress();
// the row to print on
int tachoRow = 0;
// instantiated a stopwatch class for setting up // the timer
Stopwatch sw = new Stopwatch();
// set the motor speed to 1 revolution per second
LEFT_MOTOR.setSpeed(360);
RIGHT_MOTOR.setSpeed(360);
// motors moving forward
LEFT_MOTOR.forward();
RIGHT_MOTOR.forward();
// variables for determining the maximum
// difference in tacho counts
int maxTachoCountDiff = Integer.MIN_VALUE;
int currTachoCountDiff;
// perform four repetitions of the test
for (int i = 0; i < 4; i++) {
// wait for 200 milliseconds
sw.reset();
while (sw.elapsed() < 2000)
Thread.yield();
// display the tacho counts and reset the // max, if changed
currTachoCountDiff = displayTachoCounts(tachoRow++);
if (currTachoCountDiff > maxTachoCountDiff)
maxTachoCountDiff = currTachoCountDiff;
}
// stop the motors
LEFT_MOTOR.stop();
RIGHT_MOTOR.stop();
// display the maximum difference in tacho
// counts, then wait for exit
LCD.drawString("Max diff: " + maxTachoCountDiff, 0, tachoRow);
buttons.waitForAnyPress();
}
/**
* Displays the tachometer count for each motor
*
* @param row to print the count in
*
* @return Returns the difference between the tacho
* counts of the two motors
*/
private static int displayTachoCounts(int row) {
// store the tacho counts for the two motors
int tachoCountLeft = LEFT_MOTOR.getTachoCount();
int tachoCountRight = RIGHT_MOTOR.getTachoCount();
// display the tacho counts
LCD.drawString("M1: " + tachoCountLeft + " M2: " + tachoCountRight, 0, row);
// return the difference in the tacho counts
return Math.abs(tachoCountLeft - tachoCountRight);
}
}
通过运行上面的程序，您应该会观察到类似如下的结果:
Motor 1: 711 Motor 2: 710
Motor 1: 1493 Motor 2: 1492
Motor 1: 2212 Motor 2: 2212
Motor 1: 2934 Motor 2: 2934
Max diff: 1
两个转速表读数之间的最大差值为 1，因此您可以看出两个电机实际上是同步的。
描绘一条直线
在本练习中，您需要编写一个程序，让机器人向前运行一段预定的时间(比如 10，000 毫秒)，并测量机器人行进的距离。该程序应该执行以下操作:
- 创建一个新的秒表。
- 启动两台电机 A 和 C 向前运行。
- 计算经过的时间，直到它达到 10，000 ms，在 LCD 屏幕上显示经过的时间。
- 停止两个电机。
- 计算距离与时间的比率，单位为厘米每秒。
重复你的程序三次，然后计算一个距离与时间的平均比，就是机器人的速度。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 p6.java
//Write a program to run the robot forward for some predetermined
//amount of time (say 10000 ms) and measure how far the robot went.
//******************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.utility.Stopwatch;
public class p6 {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.C);
public static void main(String[] args) {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
// block the thread until a button is pressed
buttons.waitForAnyPress();
// instantiated a stopwatch class for setting up the
// timer
Stopwatch watch = new Stopwatch();
// Begin running both motors
LEFT_MOTOR.forward();
RIGHT_MOTOR.forward();
// Clear the screen
LCD.clear();
// Reset the time on the watch
watch.reset();
// Display the elapsed time on the LCD until 10000ms
while (watch.elapsed() < 10000) {
Thread.yield();
LCD.drawString("" + watch.elapsed(), 0, 0);
}
// Stop the motors after 5 seconds
LEFT_MOTOR.stop();
RIGHT_MOTOR.stop();
buttons.waitForAnyPress();
}
}
平均而言，机器人在 10，000 毫秒内移动了 99.7 厘米，因此，机器人在以每秒 1 转的速度向前移动时，平均速度为 9.97 厘米/秒。考虑到胎面直径约为 3.1 厘米，电机速度设置为 360 度/秒，我们可以确认这一结果非常接近我们的预期；也就是 3.1 cm * 3.1415926 = 9.74 cm/s，和我们观测到的速度相当接近；即 9.97 厘米/秒。
其他运动方法
EV3 大型电机还提供了许多其他方法。表 2-6 中解释了其中一些方法:
表 2-6。
Other Methods
| 方法 | 功能 | | --- | --- | | `boolean isMoving()` | 这种方法有助于测试电机是否已完成旋转。`isMoving()`当电机因任何原因移动时，返回 true。例如，如果一个`forward()`或`backward()`方法被调用，或者一个`rotate()`任务正在进行中。 | | `int getLimitAngle()` | 返回电机当前旋转的角度，以度为单位。 | | `int getSpeed()` | 返回当前速度设置。 | | `int getRotationSpeed()` | 返回电机的当前速度，单位为每秒度数。 | | `boolean isStalled()` | 帮助您确定电机是否失速，或者电机速度调节是否失败。 | | `resetTachoCount()` | 该方法将转速表计数设置为 0，它重置调节器线程在决定何时停止旋转任务时使用的平均时间的原点。 | | `void setAcceleration(int acceleration)` | 这种方法有助于您控制电机速度从一种速度变化到另一种速度的速度。加速度以每秒的度数设置。 | | `void getAcceleration()` | 返回电机的当前加速度值，单位为每秒度数。 | | `suspendRegulation()` | 打开/关闭电机的调节。如果您想混合同一台电机的调节和非调节控制，可以使用这种方法。 |摘要
在这一章中，你学习了 EV3 电机的特点:电机如何旋转，如何设置电机的速度，以及速度调节的基本思想。具体来说，通过使用本章中提供的六个 Java leJOS 项目示例，您学习了如何编写和应用 leJOS Java 编程代码来控制和操作 Lego Mindstorms EV3 汽车。
在下一章中，你将会了解到在莱霍斯 next 的试点课程中提供的各种方法。您还将学习如何应用这些方法来控制轮式车辆，以便它可以描绘出具有预定长度的边的预定几何形状，包括正方形、三角形和六边形。
三、控制轮式车辆
本章介绍了 EV3 的 MovePilot 类中提供的各种方法。你将在飞行员课程中学习如何应用这些方法来控制轮式车辆，使其能够描绘出具有预定长度的边的预定几何形状，包括正方形、三角形和六边形。具体来说，本章包括九个示例 Java 项目，并涵盖以下主题:
- 导航 API 简介
- 使用飞行员课程的基本动作
- 使用
movepilot和differentialpilot描绘出一个正方形 - 使用
movepilot和differentialpilot描绘出一个三角形 - 使用
movepilot和differentialpilot描绘出一个六边形
导航 API 简介
你在第二章中学习了如何为 EV3 汽车创建一个简单的控制。例如，回顾以下电机测试程序，example1.java:
//************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 example1.java
//an example for motor testing
//displaying tachocount about how many degrees rotated when //pressing //ESCAPE button.
//************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
public class example1 {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
public static void main(String[] args) throws Exception {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displyang and Keys // class for buttons
Keys buttons = ev3brick.getKeys();
// block the thread until a button is pressed
buttons.waitForAnyPress();
String message = "MOTOR Testing: ";
// set up the motor A speed, i.e. 100 degrees per // second
LEFT_MOTOR.setSpeed(200);
// motor A moving forward
LEFT_MOTOR.forward();
// displaying number of degrees rotated on the
// LCD until an ESCAPE button is pressed!
while (buttons.getButtons() != Keys.ID_ESCAPE) {
LCD.clear();
LCD.drawString(message, 0, 1);
LCD.drawInt(LEFT_MOTOR.getTachoCount(), 0, 2);
Thread.sleep(1000);
LCD.refresh();
}
}
}
如example1. java所示，当开发一个 EV3 机器人程序来控制电机时，你需要指出你需要为任何动作编程哪个电机。在本例中，您使用以下指令设置电机 A 的速度:
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
LEFT_MOTOR.setSpeed(200);
After that you indicate that Motor A will turn forward using the following instruction:
LEFT_MOTOR.forward();
通常，如果您只是使用一个独特的电机，您可以使用的方法如下:
forward()backward()rotate()rotateTo()setSpeed()stop()
在实际使用中，车辆通常配备两个车轮，由两个电机独立控制和导航。这种导航是通过 leJOS 导航 API 函数实现的，通过这些函数提供了一组类和方法来控制机器人。控制车辆的 leJOS NXJ 导航类处理分层抽象。
在底层，NXTRegulatedMotor类被创建来控制转动轮子的马达。然后使用一个DifferentialPilot类来控制电机的基本运动，例如原地旋转、直线行驶或弧形行驶。
在顶层，NavPathController应用一个DifferentialPilot来移动机器人通过平面上的复杂路径。一个OdometeryPoseProvider也被用来记录机器人的位置和它前进的方向。在这样的层次结构中，控制流是基于自顶向下的方法；也就是说，路径控制器控制飞行员，然后飞行员控制马达。在这种情况下，信息流是自下而上的，飞行员从发动机收集信息来控制它们。姿态提供程序使用从飞行员处收集的里程表读数信息来更新其对机器人姿态的当前估计，包括机器人的笛卡尔坐标(即 x 轴和 y 轴)及其航向角(即基于度数的机器人面向的方向-面向正 x 轴方向为 0 度，面向正 y 轴方向为 90 度)。路径控制器使用所有这些信息来计算到目的地的距离和方向。在莱霍斯 EV3，一艘MovePilot级被用来代替DifferentialPilot。然而，基本思想是相同的。
使用飞行员课程的基本动作
这一章将介绍使用飞行员课程的基本动作，这是导航中最重要的步骤之一。正如你在第二章中看到的，仅仅使用马达类就可以移动机器人:也就是说，简单地通过向前和向后旋转马达。然而，为了驾驶到特定的位置(也就是说，进行精确的移动)，有必要控制车辆向前或向后移动的距离以及机器人转动的角度。因此，使用 pilot 进行基本移动的主要目标是创建可以执行精确移动的车辆。pilot 类用于精确驾驶、驾驶和转向车辆，包括直线行驶、原地旋转、弧线和停止。
通过将这些运动的序列打包在一起，机器人可以通过重复执行两个基本步骤的组合从一个位置行进到另一个位置，例如机器人向前或向后行进的量以及机器人在整个运动中顺时针和逆时针旋转的量。
使用 pilot 类的一个好处是，除了飞行员，机器人的实际物理特征都隐藏在 navigator 包中。从内部运动的角度来看，机器人可以滚动、行走、跳跃或从一个位置飞到另一个位置。然而，从外部的角度来看，你只看到外部的飞行员方法，使机器人的动作看起来都一样。因此，使用 pilot 允许您对不同类型的机器人进行编程以参与导航，而不管它们的物理结构如何。
在 leJOS 中，一个Move类被用来告诉机器人做什么动作，或者指示机器人刚刚做了什么样的动作。Move类的一些核心方法包括:
getDistanceTraveled ()获取车辆移动的距离，通常以厘米为单位。getTurnAngle ()获取车辆在运动中旋转的角度，以度为单位。getArcRadius ()接收车辆行驶的圆弧半径。
接下来，您将了解如何使用物理机器人来执行这些动作。在 leJOS 中，pilot 是一个控制特定机器人的类。特别是DifferentialPilot级可以控制两个轮子的机器人。在这一章中，您将使用差动电机控制，因为它能够进行所有的移动，而且为它制造底盘也很简单。DifferentialPilot级通过控制发动机的速度和旋转方向来驾驶车辆。飞行员对象需要知道马达连接到哪个端口，以及向前驱动马达是否使机器人向前或向后移动。该对象还需要知道车轮的直径和轨道的宽度:即两个车轮的轨道中心之间的距离。原因是DifferentialPilot类使用车轮直径来计算它行驶的距离，使用轮距来计算它旋转了多远。所有这些信息都将传递给 pilot 构造函数，如以下代码行所示:
DifferentialPilot(float wheelDiameter, float trackWidth, Motor leftMotor, Motor rightMotor)
否则，使用以下构造函数将布尔变量 reverse 设置为 true，这样电机将向后旋转，使机器人向前移动:
DifferentialPilot(float wheelDiameter, float trackWidth, Motor leftMotor, Motor rightMotor, boolean reverse)
总之，为了控制机器人沿直线运动，可以使用以下方法:
void setTravelSpeed(double travelSpeed)以相同的距离单位设置电机的速度(例如，如果车轮的直径以厘米为单位测量，则该方法设置的数值将以厘米/秒为单位)void forward()启动机器人向前移动void backward()启动机器人向后移动void stop()停止机器人移动void rotate (double angle)将机器人旋转一定角度
为了控制机器人移动的距离，可以使用以下方法:
void travel(double distance)以与车轮直径相同的单位将电机移动指定的距离(即，如果车轮直径以厘米为单位，则该方法设置的数值将以厘米为单位)getMovement().getDistanceTraveled()返回车辆行驶的距离
例如，下面的example2.java展示了如何使用 leJOS NXJ 中的DifferentialPilot类让机器人移动 20 厘米。
//****************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 example2.java
//an example for making basic movement using DifferentialPilot //in leJOS NXJ
//****************************************************************************
import lejos.nxt.*;
import lejos.robotics.navigation.*;
public class example2 {
public static void main(String[] args) {
DifferentialPilot pilot = new DifferentialPilot(3.1, 17.5, Motor.A,Motor.C);
pilot.travel(20, true
); // 20 centimeters
pilot.rotate(90.0);
while (pilot.isMoving()) {
if (Button.ESCAPE.isPressed())
pilot.stop();
Button.waitForPress();
}
}
}
如example2.java所示，您可以使用以下命令使机器人原地旋转指定的角度:
void rotate(double degrees)
此外，当使用这种方法产生精确的运动时，您需要精确地测量wheelDiameter和trackWidth的值。直径是从圆的一边到另一边的最宽的测量值。根据我的测量，我的机器人上的轮子直径是 3.1 厘米。为了在机器人转弯时记录精确的旋转，了解轮与轮之间的距离也很重要，即轮距。由于乐高轮胎是对称的，最好的方法是从一个轮胎的中心到另一个轮胎的中心进行测量。根据我的测量，上面例子中的轨道宽度是 17.5 厘米。DifferentialPilot的最终参数是左右轮的电机。在上面的示例程序中，左马达连接到端口 A，右马达连接到端口 c。
由于MovePilot在新的莱霍斯 EV3 中被用来代替DifferentialPilot，下面的例子example3.java提供给你一个在莱霍斯 EV3 使用MovePilot的基本运动控制。
//************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 example3.java
//an example for making basic movement using MovePilot in //leJOS EV3
//************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.*;
public class example3 {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.C);
public static void main(String[] args) throws Exception {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys // class for buttons
Keys buttons = ev3brick.getKeys();
// block the thread until a button is pressed
buttons.waitForAnyPress();
// setup the wheel diameter of left (and right) motor // in centimeters, i.e. 2.8 cm
// the offset number is the distance between the center // of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot pilot = new MovePilot(chassis);
// travel 100 centimeter
pilot.travel(100);
// rotate 90 degrees
pilot.rotate(90.0);
// press the ESCAPE button to stop moving
while (pilot.isMoving()) {
if (buttons.getButtons() == Keys.ID_ESCAPE)
pilot.stop();
}
// block the thread until a button is pressed
buttons.waitForAnyPress();
}
}
画出一个正方形
在本节中，您将开发一个程序来创建一个机器人，该机器人描绘出一个边长预定为 1 米的正方形。您将编写、编译并上传您的代码到 Lego Mindstorms。此外，你还需要计算出电机向前移动 N 厘米的距离需要多长时间；例如，在这种情况下，N 被设置为 100 厘米。此外，您将计算出电机旋转 90 度(正方形的角)需要多长时间。
一个示例程序example4.java，展示了如何在 EV3 的莱霍斯使用MovePilot描绘出一个正方形。
//************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 example4.java
//an example for tracing out a square
//************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.*;
public class example4 {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.C);
public static void main(String[] args) throws Exception {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys // class for buttons
Keys buttons = ev3brick.getKeys();
// block the thread until a button is pressed
buttons.waitForAnyPress();
// setup the wheel diameter of left (and right) // motor in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the // center of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 =
WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot pilot = new MovePilot(chassis);
// loop 4 times to trace out a square
for (int sides = 0; sides < 4; sides++) {
// travel 100 centimeter
pilot.travel(100);
// rotate 90 degrees
pilot.rotate(90);
}
// press the ESCAPE button to stop moving
while (pilot.isMoving()) {
if (buttons.getButtons() == Keys.ID_ESCAPE)
pilot.stop();
}
// block the thread until a button is pressed
buttons.waitForAnyPress();
}
}
如果您使用 leJOS NXJ，下面的示例程序example5.java将说明如何使用DifferentialPilot类中提供的一组方法描绘出一个正方形。这背后的想法是基于基准测试，该测试显示车辆可以以 30.76 厘米/秒的速度向前行驶。相应地，车辆需要行驶 3.25 秒来完成正方形的一边。为了实现这一点，我们应用了一个sleep函数，该函数从机器人向前移动开始，等待 3.25 秒，然后停止。评估程序的实验结果表明，该算法比直接使用DifferentialPilot类的方法更有效。
//***************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 example5.java
//an example for tracing out a square using DifferentialPilot in //leJOS NXJ
//***************************************************************************
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.NXTRegulatedMotor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.util.Stopwatch;
public class example5 {
// The vehicle's rate of travel forward in centimeters per
// second
private static final double FW_SPEED = 30.666666667f;
// The vehicle's clockwise rotation speed in degrees per
// second
private static final double ROTATION_SPEED = 248.297752809f;
// The vehicle's wheel-width.
private static final double WHEEL_DIAMETER = 3.1;
// The vehicle's width
private static final double TRACK_WIDTH = 17.5;
// Reference to left motor
private static NXTRegulatedMotor LEFT_MOTOR = Motor.A;
// Reference to right motor
private static NXTRegulatedMotor RIGHT_MOTOR = Motor.C;
/**
* The main entry point for the program.
*/
public static void main(String[] args) {
// construct the pilot using the static variables
DifferentialPilot pilot = new DifferentialPilot(WHEEL_DIAMETER,
TRACK_WIDTH, LEFT_MOTOR, RIGHT_MOTOR, false);
// tracing out a square
drawSquare(pilot, 100);
LCD.drawString("tracing is done, press button to exit.", 0, 0);
Button.waitForPress();
}
/**
* Draws a square of the specified side length
*
* @param pilot
* The pilot to use when drawing the square
* @param sideLength
* The length of each leg of the square
*/
public static void drawSquare(DifferentialPilot pilot, double sideLength) {
for (int i = 0; i < 4; i++) {
// delay 500ms to allow the motors to stabilize
sleep(500);
// move the vehicle forward
forward(pilot, sideLength);
// delay 500ms to allow the motors to stabilize
sleep(500);
// rotate the vehicle by 90 degrees to prepare
// for
// the next side of the square
rotate(pilot, 90);
}
}
/**
* Move the vehicle forward the specified distance
*
* @param pilot
* The pilot to use for moving forward
* @param distance
* The distance to travel
*/
public static void forward(DifferentialPilot pilot, double distance) {
// get the number of milliseconds the vehicle should
// travel based on the
// vehicle's speed
int travelTime = getMillisForTravel(distance);
// begin the pilot forward
pilot.forward();
// wait for the traveling to finish
sleep(travelTime);
// reached the destination - stop
pilot.stop();
}
/**
* Rotate the vehicle by the specified angle. Positive angles * will result in
* a clockwise rotation. Negative angles will result in a
* counterclockwise
* rotation.
*
* @param pilot
* @param angle
* The angle (in degrees) by which to rotate the
* vehicle
*/
public static void rotate(DifferentialPilot pilot, double angle) {
// determine the number of milliseconds to rotate based // on the vehicle
// speed
int travelTime = getMillisForRotate(angle);
// for negative angles, rotate counterclockwise
if (angle < 0)
pilot.rotateLeft();
else
// for positive angles, rotate clockwise
pilot.rotateRight();
// block the thread until the motion is complete
sleep(travelTime);
pilot.stop();
}
/**
* Gets the milliseconds to travel for a given travel distance
*
* @param distance
* The distance in centimeters to travel
* @return Returns the number of milliseconds to travel
*/
public static int getMillisForTravel(double distance) {
return (int) ((distance / FW_SPEED) * 1000);
}
/**
* Gets the milliseconds to rotate for the specified number of * degrees
*
* @param rotateDegree
* @return the number of milliseconds to rotate
*/
public static int getMillisForRotate(double rotateDegree) {
return (int) ((rotateDegree / ROTATION_SPEED) * 1000);
}
/**
* Sleep function using Thread.yield rather than thread.sleep
*
* @param millis
* The number of milliseconds to block the
* executing thread
*/
public static void sleep(long millis) {
// create the stopwatch
Stopwatch sw = new Stopwatch();
// continue waiting while the elapsed time is less than // the time
// specified
while (sw.elapsed() < millis)
Thread.yield();
}
}
描绘出一个等边三角形
在本节中，您将开发一个程序来创建一个机器人，该机器人可以描绘出一个等边三角形，其预定边长设置为 1 米。您将编写、编译并上传您的代码到 Lego Mindstorms。一个示例程序，example6.java，展示了如何在 EV3 的莱霍斯使用MovePilot描绘出一个等边三角形。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 example6.java
//an example for tracing out an equilateral triangle
//******************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.*;
public class example6 {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.C);
public static void main(String[] args) throws Exception {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
// block the thread until a button is pressed
buttons.waitForAnyPress();
// setup the wheel diameter of left (and right) motor // in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center // of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot pilot = new MovePilot(chassis);
// loop 3 times to trace out an equilateral triangle
for (int sides = 0; sides < 3; sides++) {
// travel 100 centimeter
pilot.travel(100);
// rotate 120 degrees
pilot.rotate(120);
}
// press the ESCAPE button to stop moving
while (pilot.isMoving()) {
if (buttons.getButtons() == Keys.ID_ESCAPE)
pilot.stop();
}
// block the thread until a button is pressed
buttons.waitForAnyPress();
}
}
如果您使用 leJOS NXJ，下面的示例程序example7.java将说明如何使用DifferentialPilot类中提供的一组方法描绘出一个三角形。
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 example7.java
//an example for tracing out an equilateral triangle using //DifferentialPilot in leJOS NXJ
//******************************************************************************************
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.NXTRegulatedMotor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.util.Stopwatch;
public class example7 {
// The vehicle's rate of travel forward in centimeters per
// second
private static final double FW_SPEED = 30.666666667f;
// The vehicle's clockwise rotation speed in degrees per
// second
private static final double ROTATION_SPEED = 248.297752809f;
// The vehicle's wheel-width.
private static final double WHEEL_DIAMETER = 3.1;
// The vehicle's track width
private static final double TRACK_WIDTH = 17.5;
// Reference to left motor
private static NXTRegulatedMotor LEFT_MOTOR = Motor.A;
// Reference to right motor
private static NXTRegulatedMotor RIGHT_MOTOR = Motor.C;
/**
* The main entry point for the program.
*/
public static void main(String[] args) {
// construct the pilot using the static variables
DifferentialPilot pilot = new DifferentialPilot(WHEEL_DIAMETER,
TRACK_WIDTH, LEFT_MOTOR, RIGHT_MOTOR, false);
// tracing out a triangle
drawTriangle(pilot, 100);
LCD.drawString("tracing is done, press button to exit.", 0, 0);
Button.waitForPress();
}
/**
* Draw an equilateral triangle
*
* @param pilot
* The pilot to use to draw the triangle
* @param sideLengthCm
* The side length of the triangle
*/
public static void drawTriangle(DifferentialPilot pilot, double sideLength) {
// iterate through the sides of the triangle
for (int i = 0; i < 3; i++) {
sleep(500);
// travel along a side
forward(pilot, sideLength);
sleep(500);
// re-orient to travel along the next side
rotate(pilot, 120);
}
}
/**
* Move the vehicle forward the specified distance
*
* @param pilot
* The pilot to use for moving forward
* @param distance
* The distance to travel
*/
public static void forward(DifferentialPilot pilot, double distance) {
// get the number of milliseconds the vehicle should
// travel based on the
// vehicle's speed
int travelTime = getMillisForTravel(distance);
// begin the pilot forward
pilot.forward();
// wait for the traveling to finish
sleep(travelTime);
// reached the destination - stop
pilot.stop();
}
/**
* Rotate the vehicle by the specified angle. Positive angles
* will result in
* a clockwise rotation. Negative angles will result in a
* counterclockwise
* rotation.
*
* @param pilot
* @param angle
* The angle (in degrees) by which to rotate the vehicle
*/
public static void rotate(DifferentialPilot pilot, double angle) {
// determine the number of milliseconds to rotate based
// on the vehicle
// speed
int travelTime = getMillisForRotate(angle);
// for negative angles, rotate counterclockwise
if (angle < 0)
pilot.rotateLeft();
else
// for positive angles, rotate clockwise
pilot.rotateRight();
// block the thread until the motion is complete
sleep(travelTime);
pilot.stop();
}
/**
* Gets the milliseconds to travel for a given travel distance
*
* @param distance
* The distance in centimeters to travel
* @return Returns the number of milliseconds to travel
*/
public static int getMillisForTravel(double distance) {
return (int) ((distance / FW_SPEED) * 1000);
}
/**
* Gets the milliseconds to rotate for the specified number of
* degrees
*
* @param rotateDegree
* @return the number of milliseconds to rotate
*/
public static int getMillisForRotate(double rotateDegree) {
return (int) ((rotateDegree / ROTATION_SPEED) * 1000);
}
/**
* Sleep function using Thread.yield rather than thread.sleep
*
* @param millis
* The number of milliseconds to block the executing thread
*/
public static void sleep(long millis) {
// create the stopwatch
Stopwatch sw = new Stopwatch();
// continue waiting while the elapsed time is less than // the time
// specified
while (sw.elapsed() < millis)
Thread.yield();
}
}
描绘出一个正六边形
在本节中，您将开发一个程序来创建一个机器人，该机器人可以描绘出一个边长预定为 50 厘米的正六边形。您将编写、编译并上传您的代码到 Lego Mindstorms。一个示例程序example8.java，展示了如何在 EV3 的莱霍斯使用MovePilot描绘出一个正六边形。
//************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 example8.java
//an example for tracing out a regular hexagon
//************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.*;
public class example8 {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.C);
public static void main(String[] args) throws Exception {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys // class for buttons
Keys buttons = ev3brick.getKeys();
// block the thread until a button is pressed
buttons.waitForAnyPress();
// setup the wheel diameter of left (and right) // motor in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the // center of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential
// pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot pilot = new MovePilot(chassis);
// loop 6 times to trace out a regular hexagon
for (int sides = 0; sides < 6; sides++) {
// travel 100 centimeter
pilot.travel(50);
// rotate 60 degrees
pilot.rotate(60);
}
// press the ESCAPE button to stop moving
while (pilot.isMoving()) {
if (buttons.getButtons() == Keys.ID_ESCAPE)
pilot.stop();
}
// block the thread until a button is pressed
buttons.waitForAnyPress();
}
}
如果你使用 leJOS NXJ，这里提供了一个示例程序example9. java来说明如何使用DifferentialPilot类中提供的一组方法描绘出一个六边形。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 example9.java
//an example for tracing out a regular hexagon using //DifferentialPilot in leJOS NXJ
//******************************************************************
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.NXTRegulatedMotor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.util.Stopwatch;
public class example9 {
// The vehicle's rate of travel forward in centimeters per
// second
private static final double FW_SPEED = 30.666666667f;
// The vehicle's clockwise rotation speed in degrees per
// second
private static final double ROTATION_SPEED = 248.297752809f;
// The vehicle's wheel-width.
private static final double WHEEL_DIAMETER = 3.1;
// The vehicle's wheel-bas
private static final double TRACK_WIDTH = 17.5;
// Reference to left motor
private static NXTRegulatedMotor LEFT_MOTOR = Motor.A;
// Reference to right motor
private static NXTRegulatedMotor RIGHT_MOTOR = Motor.C;
/**
* The main entry point for the program.
*/
public static void main(String[] args) {
// construct the pilot using the static variables
DifferentialPilot pilot = new DifferentialPilot(WHEEL_DIAMETER,
TRACK_WIDTH, LEFT_MOTOR, RIGHT_MOTOR, false);
// tracing out a triangle
drawHexagon(pilot, 50);
LCD.drawString("tracing is done, press button to exit.", 0, 0);
Button.waitForPress();
}
/**
* Draw a hexagon with sides of the specified length
*
* @param pilot
* The pilot to use for drawing the hexagn
* @param sideLengthCm
* The length of each side of the regular hexagon
*/
public static void drawHexagon(DifferentialPilot pilot, double sideLength) {
// iterate through the sides of the hexagon
for (int i = 0; i < 6; i++) {
sleep(500);
// move the vehicle along an edge
forward(pilot, sideLength);
sleep(500);
// rotate the bot to traverse the next leg
rotate(pilot, 60);
}
}
/**
* Move the vehicle forward the specified distance
*
* @param pilot
* The pilot to use for moving forward
* @param distance
* The distance to travel
*/
public static void forward(DifferentialPilot pilot, double distance) {
// get the number of milliseconds the vehicle should
// travel based on the
// vehicle's speed
int travelTime = getMillisForTravel(distance);
// begin the pilot forward
pilot.forward();
// wait for the traveling to finish
sleep(travelTime);
// reached the destination - stop
pilot.stop();
}
/**
* Rotate the vehicle by the specified angle. Positive angles
* will result in
* a clockwise rotation. Negative angles will result in a
* counterclockwise
* rotation.
*
* @param pilot
* @param angle
* The angle (in degrees) by which to rotate the vehicle
*/
public static void rotate(DifferentialPilot pilot, double angle) {
// determine the number of milliseconds to rotate based // on the vehicle
// speed
int travelTime = getMillisForRotate(angle);
// for negative angles, rotate counterclockwise
if (angle < 0)
pilot.rotateLeft();
else
// for positive angles, rotate clockwise
pilot.rotateRight();
// block the thread until the motion is complete
sleep(travelTime);
pilot.stop();
}
/**
* Gets the milliseconds to travel for a given travel distance
*
* @param distance
* The distance in centimeters to travel
* @return Returns the number of milliseconds to travel
*/
public static int getMillisForTravel(double distance) {
return (int) ((distance / FW_SPEED) * 1000);
}
/**
* Gets the milliseconds to rotate for the specified number of
* degrees
*
* @param rotateDegree
* @return the number of milliseconds to rotate
*/
public static int getMillisForRotate(double rotateDegree) {
return (int) ((rotateDegree / ROTATION_SPEED) * 1000);
}
/**
* Sleep function using Thread.yield rather than thread.sleep
*
* @param millis
* The number of milliseconds to block the
* executing thread
*/
public static void sleep(long millis) {
// create the stopwatch
Stopwatch sw = new Stopwatch();
// continue waiting while the elapsed time is less than // the time
// specified
while (sw.elapsed() < millis)
Thread.yield();
}
}
摘要
在这一章中，你学习了 leJOS EV3 的MovePilot类和 leJOS NXJ 的DifferentialPilot类中提供的各种方法。基于这些方法，你现在知道如何精确地控制轮式车辆的运动。具体来说，九个 Java leJOS 示例项目详细说明了如何使用 Lego Mindstorms EV3 和 Lego Mindstorms NXT 的 pilot 类描绘出具有预定义长度的边的预定义几何形状。
在下一章中，你将学习到在 EV3 使用的笛卡尔坐标系的基础知识。然后，在此基础上，你将学习如何在导航课程中应用编程方法来控制轮式车辆，以便在二维平面中用坐标描绘出预定义的路径。此外，还将介绍乐高 EV3 积木的主要硬件组件。
四、协调器和导航器 API
这一章向你介绍了在莱霍斯 EV3 使用的笛卡尔坐标系的基础知识。它还教你如何在导航课程中应用编程方法来控制轮式车辆，以便在二维平面中用坐标描绘出预定义的路径。此外，您将了解乐高 EV3 积木的主要硬件组件，如液晶显示器和与机器人互动的按键。您还将学习如何应用用于控制 LCD 显示器的方法以及用于向机器人提供输入和从机器人获得输出的按钮。
特别是，本章包括七个 Java 项目示例，并涵盖以下主题:
- 笛卡尔坐标系简介
- 导航器 API 函数的基础
- 控制 EV3 砖硬件
- 按钮和液晶显示器编程练习
- 追踪二维平面的编程实践
笛卡尔坐标系基础
从我们的角度来看，我们可以很容易地用诸如“我位于第 10 大道和第 2 街的拐角处”或“我位于新罕布什尔州基恩市主街 100 号”这样的词语来描述位置然而，这样的描述对乐高机器人没有任何意义，因为它们没有语义概念。相反，乐高机器人只理解数字。
因此，当编程乐高机器人时，笛卡尔坐标系被用来描述位置。如图 4-1 所示，一个二维笛卡尔坐标系记录两个数字:X 和 Y 的值，数字沿着 X 轴和 Y 轴变大变小。两个轴都从 0 开始，包含正数和负数。X 轴和 Y 轴将笛卡尔坐标系分成四个象限，即区域 I、II、III 和 IV。二维区域中的任何一点都可以用 X 和 y 的值绘制在这个网格上。
图 4-1。
A Cartesian coordinate system
此外，在笛卡尔坐标系中，向左旋转被指定为正旋转。这意味着，如果你旋转正 90 度(+90)，你已经逆时针旋转。同样，负 90 度(-90°)的旋转相当于顺时针方向旋转。
导航 API 函数
在第三章中，你学习了飞行员，比如MovePilot和DifferentialPilot，以及飞行员如何让机器人执行精确的动作和行驶特定的距离。使用 pilot，您将了解另一个名为 Navigator 的类，它告诉飞行员如何根据笛卡尔坐标系驾驶到特定的位置。
导航器在其构造函数中接受一个 pilot 对象，然后计算从一个位置到另一个位置的一系列移动。导航者实际上对机器人如何工作或者飞行员物体如何四处移动一无所知。取而代之的是，导航器只是要求飞行员执行运动指令。
下面的代码语句实例化一个导航器，然后行驶到目标坐标 x = 50，y = 50，其中我们假设存在一个导航器:
Navigator navtest = new Navigator(pilot);
navtest.goTo(50,50);
正如您在第三章中了解到的，您可以使用以下程序构建一个 pilot 对象:
// setup the wheel diameter of left (and right) motor in
// centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center of
// wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 }, WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot pilot = new MovePilot(chassis);
在导航类中，目标坐标也是已知的航路点。您可以创建自己的方法来生成一组路点，然后使用路点对象将坐标提供给导航器。以下内容显示了如何使用该航路点:
Waypoint wp = new Waypoint (50, 50);
navtest.goTo(wp);
在查找路径的情况下，一旦找到路径，您就可以使用addWaypoint()方法将路点放入导航队列。例如，
navtest.addWaypoint(new Waypoint(200,200));
导航器驱动导航对象进行移动控制。这是通过PoseProvider来实现的，它跟踪机器人在一条路线上的位置顺序。route 是 path 类的一个实例，它表现为一个先进先出(FIFO)队列。当到达一个航路点时，它会从路线队列中删除，机器人会转到下一个航路点。在任何时候，新的路点都可以添加到路线队列的末尾。
下面是导航类中的方法列表。导航类提供的所有方法都是非阻塞的，这意味着这些方法在完成它们的功能后会立即返回。
void setPath(Path path)设置导航器遍历的路径。void addWaypoint(Waypoint waypoint)在路径末端添加一个航路点。void addWaypoint(float x, float y )构建一个坐标为(x，y)的新航路点，然后将其添加到路径末端。void addWaypoint(float x, float y , heading )创建一个坐标为(x，y)的新航路点，然后将其添加到路径末端。当机器人到达航路点时，您可以使用此方法指定机器人的方向。void followPath()启动机器人沿现有/当前路线移动。void followPath(Path path )启动机器人遍历要跟随的路径。void goTo(Waypoint destination)启动机器人向目的地移动。如果不存在路径，则创建一个包含目的地的新路径；否则，目标将被添加到现有路径中。void goTo(double x, double y)启动机器人向坐标(x，y)指定的目的地移动。如果不存在路径，则创建一个包含目的地的新路径；否则，新的航路点将被添加到现有路径中。void goTo(double x, double y, double heading)启动机器人向坐标(x，y)指定的目的地移动。如果不存在路径，则创建一个包含目的地的新路径；否则，新的航路点将被添加到现有路径中。当机器人到达目的地航路点时，您可以使用此方法指定机器人的方向。leJOSvoid stop()停止机器人，但保留路线，以便您可以在调用followPath()时恢复其路径遍历。- 如果机器人正在向一个航路点移动，返回 true。
- 如果机器人已经到达最后一个航路点，返回 true。
void rotateTo(double direction)将机器人旋转到笛卡尔平面中新的方向角度。例如，当 x 轴为 0 时，rotateTo(0)会将机器人与 x 轴对齐。如果 y 轴是 90 度，rotateTo(90)将其与 y 轴对齐。方向的数值是机器人可以旋转到的绝对方向，范围从 0 到 360。void waitForStop()如果机器人停在路径的最后一个航路点并且不再移动，返回 true。void singleStep(boolean yes)控制机器人是否停在每个航路点。如果yes，你可以用真参数调用这个方法，然后机器人停在每个航点。
下面是一个简单的例子，ch4p1.java，将你的机器人从起点(0，0)移动到坐标为(50，50)的位置。这里的测量单位是厘米，因为 pilot 使用的直径和轨道宽度是厘米(例如，2.8 厘米和 18 厘米)。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch4p1.java
//an example for navigation testing
//******************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.*;
public class ch4p1 {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.C);
public static void main(String[] args) throws Exception {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
// block the thread until a button is pressed
buttons.waitForAnyPress();
// setup the wheel diameter of left (and right) motor // in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center // of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot pilot = new MovePilot(chassis);
Navigator navtest = new Navigator(pilot);
// define a new waypoint as destination
Waypoint wp = new Waypoint (50, 50);
// robot moves to the destination waypoint
navtest.goTo(wp);
// block the thread until a button is pressed
buttons.waitForAnyPress();
}
}
控制 EV3 砖硬件
EV3 砖配备了 6 节 AA 电池，或稍大的可充电电池组。这种耐用的砖块还包含一个 32 位 ARM9 处理器，运行频率为 300MHz。该处理器可直接访问 64MB RAM 和 16MB 闪存。为了延长电池寿命，即使没有电源输入，闪存也能存储数据和程序。与拥有 4GB 内存的现代计算机相比，乐高 EV3 使用的内存非常小。然而，这对您的机器人项目来说实际上已经足够了，因为现代计算机消耗了大量的 RAM 容量来运行操作系统、生成图形和执行其他内存密集型任务。然而，机器人程序不使用重型图形或执行处理器密集型计算任务。
在本节中，您将熟悉 EV3 砖，包括用于输入的按钮、液晶显示器(LCD)和用于输出的小型扬声器。莱霍斯 EV3 公司提供用于控制所有硬件的 API(应用接口)功能。
莱霍斯 EV3 的 LCD 类可用于文本模式或图形模式。对于文本显示，EV3 液晶显示屏宽 16 个字符，深 8 个字符。用坐标(x，y)来寻址，如图 4-2 所示，其中 x 的范围是 0 到 15，y 的范围是 0 到 7。
图 4-2。
Coordinate system of LCD display
在文本模式下写入 LCD 屏幕的方法如下:
void drawString(String str, int x, int y)从坐标(x，y)开始，在 LCD 屏幕上显示一串文本。void drawInt(int i, int x, int y)显示从坐标(x，y)开始的整数。整数是左对齐的，根据需要占用尽可能多的字符。void drawInt(int i, int places, int x, int y)显示一个从坐标(x，y)开始的整数，其前导空格至少占据 places 参数指定的字符数。这意味着该方法总是写入位置中指定的固定数量的字符，并且以前的值将总是被完全覆盖。例如，如果 places 的值设置为 5，这意味着在 LCD 上显示整数时有 5 个字符要覆盖。void clear()清除显示。
作为一个例子，程序ch2p2. java说明了如何在 EV3 砖的 LCD 上显示空闲存储空间:
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch4p2.java
//An example to test LCD displaying methods
//******************************************************************
// import EV3 hardware packages for EV brick finding, activating
// keys and LCD
import lejos.hardware.ev3.EV3;
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.lcd.TextLCD;
public class ch4p2 {
public static void main(String[] args) throws InterruptedException {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantized LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
TextLCD lcddisplay = ev3brick.getTextLCD();
// drawing text on the LCD screen based on coordinates
lcddisplay.drawString("Free RAM:", 0, 0);
// displaying free memory on the LCD screen
lcddisplay.drawInt((int) Runtime.getRuntime().freeMemory(), 0, 1);
// exit program after any button pressed
buttons.waitForAnyPress();
}
}
您也可以用System.out.println(String str)写入 LCD 显示屏。例如，System.out.println("hello")将在屏幕上显示字符串hello，并覆盖现有字符。默认情况下，LCD 显示屏会自动刷新。如果你想控制 LCD 何时刷新，你可以调用lcddisplay.setAutoRefresh(false)关闭自动刷新，当你想刷新显示时调用lcddisplay.refresh()。
EV3 砖块上的所有六个控制键都可以在莱霍斯 keys 的控制下重新编程。您可以使用事件来监听按键，并在按键被激活时做出相应的反应。Keys类包含六个键的静态实例。这六个实例是ENTER、ESCAPE、LEFT、RIGHT、UP和DOWN。
通常，在编程时，您希望等待或阻止进程，直到某个键被按下。因此，最简单的方法就是使用waitForAnyPress()方法。例如，要停止代码直到按下ESCAPE键，您可以执行以下操作:
buttons.getButtons()==Keys.ID_ESCAPE
您还可以使用一个简单的 while 循环来停止您的代码，同时等待用户按下一个ESCAPE按钮:
while(buttons.getButtons() != Keys.ID_ESCAPE) { }
以下程序ch4p3.java测试按钮是否被按下:
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch4p3.java
//an example for button testing
//******************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
public class ch4p3 {
public static void main(String[] args) throws Exception {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// Keys class for buttons
Keys buttons = ev3brick.getKeys();
// press the ESCAPE key to exit the program
while (buttons.getButtons() != Keys.ID_ESCAPE) {
// clearing the LCD screen at first
LCD.clear();
// press the ENTER key so the ENTER will be
// displayed on the LCD screen
if (buttons.getButtons() == Keys.ID_ENTER) {
// displaying ENTER on the LCD screen
LCD.drawString("ENTER", 0, 0);
// leave the string ENTER on the screen
// for 2 seconds
Thread.sleep(2000);
}
}
}
}
用于控制按键的其他方法包括:
static int waitForAnyPress()
要等待任何键被按下，上面的命令返回被按下的键的 id 代码，如下所示:
static int readButtons()读取所有按键的当前状态。返回值是被按下的键的所有代码的总和。
如上面的程序ch4p3.java所示，你会发现Thread.sleep(2000)是用来加入各种延迟，以便机器人可以休息去做其他事情。在许多情况下，当使用传感器(例如，光传感器或颜色传感器)记录需要极高精度的快速事件时，会发生这种情况。这样的事件可能是等待颜色传感器检测黑线，或者等待超声波传感器检测墙壁。因此，方法Thread.sleep()可以用于为不同的动作计时或者为事件打上时间戳，以供以后分析。
液晶显示器编程实践
在本练习中，您将开发一个程序，在 LCD 屏幕上显示“Here is my RAM”五秒钟，然后打印出“I got it.”。一个示例程序ch4p4.java说明了如何实现这一目标。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch4p4.java
//A programming practice example to display free RAM on LCD
//******************************************************************
// import EV3 hardware packages for EV brick finding, activating
// keys and LCD
import lejos.hardware.ev3.EV3;
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.lcd.TextLCD;
import lejos.utility.Stopwatch;
public class ch4p4 {
public static void main(String[] args) throws InterruptedException {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// LCD class for displaying and Keys class for buttons
Keys buttons = ev3brick.getKeys();
TextLCD lcddisplay = ev3brick.getTextLCD();
// for timing dialogs
Stopwatch sw = new Stopwatch();
// drawing free RAM on the LCD screen based on
// coordinates
lcddisplay.drawString("Here is my RAM:", 0, 0);
// displaying free memory on the LCD screen
lcddisplay.drawInt((int) Runtime.getRuntime().freeMemory(), 0, 1);
sw.reset();
// wait for 5 seconds, then display a message
while (sw.elapsed() < 5000)
Thread.yield();
sw.reset();
lcddisplay.drawString("I got it", 0, 2);
// exit program after any button pressed
buttons.waitForAnyPress();
}
}
按键编程练习
在本编程练习课中，您将开发两个程序来按键并在 LCD 上显示相应的消息。在第一个程序中，您需要在 LCD 上显示被按下的按钮。其伪代码如下例所示:
while(ESCAPSE is not pressed){
Clear LCD
If ENTER is pressed
Display “ENTER” on first row
Else if LEFT is pressed
Display “LEFT” on the first row
else if RIGHT is pressed
Display “RIGHT” on the first row
else if UP is pressed
Display “UP” on the first row
else if DOWN is pressed
Display “DOWN” on the first row
}
一个示例程序ch4p5.java向您展示了如何完成这个练习。
//************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch4p5.java
//A programming practice example to display various buttons when //they are pressed
//************************************************************************************
// import EV3 hardware packages for EV brick finding, activating
// keys and LCD
import lejos.hardware.ev3.EV3;
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.lcd.TextLCD;
public class ch4p5 {
public static void main(String[] args) throws InterruptedException {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// LCD class for displaying and Keys class for buttons
Keys buttons = ev3brick.getKeys();
TextLCD lcddisplay = ev3brick.getTextLCD();
// continue waiting for button pressed while the user // has not pressed
// escape button
while (buttons.getButtons() != Keys.ID_ESCAPE) {
// display a message for enter
if (buttons.getButtons() == Keys.ID_ENTER) {
lcddisplay.clear();
lcddisplay.drawString("ENTER", 0, 0);
}
// display a message for left button
else if (buttons.getButtons() == Keys.ID_LEFT) {
lcddisplay.clear();
lcddisplay.drawString("LEFT", 0, 0);
}
// display a message for right button
else if (buttons.getButtons() == Keys.ID_RIGHT) {
lcddisplay.clear();
lcddisplay.drawString("RIGHT", 0, 0);
}
// display a message for up button
else if (buttons.getButtons() == Keys.ID_UP) {
lcddisplay.clear();
lcddisplay.drawString("UP", 0, 0);
}
// display a message for down button
else if (buttons.getButtons() == Keys.ID_DOWN) {
lcddisplay.clear();
lcddisplay.drawString("DOWN", 0, 0);
}
}
}
}
在第二个练习中，您需要编写一个程序，在执行程序的其余部分之前等待ENTER被按下。下面的示例说明了伪代码:
Clear LCD
Display "Press ENTER to continue" then Refresh LCD
Wait for the ESCAPE to be pressed and released
Display a message that the ESCAPE is indeed pressed and released
一个示例程序ch4p6.java向您展示了如何完成这个练习。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch4p6.java
//A programming practice example to display ESCAPE button pressed
// and released
//******************************************************************
// import EV3 hardware packages for EV brick finding, activating
// keys and LCD
import lejos.hardware.ev3.EV3;
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.lcd.TextLCD;
import lejos.utility.Stopwatch;
public class ch4p6 {
public static void main(String[] args) throws InterruptedException {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// LCD class for displaying and Keys class for buttons
Keys buttons = ev3brick.getKeys();
TextLCD lcddisplay = ev3brick.getTextLCD();
// print instructions for the user
lcddisplay.clear();
lcddisplay.drawString("Prs ENTER to cont", 0, 0);
lcddisplay.refresh();
// wait until ENTER key is pressed
while (buttons.waitForAnyPress() != Keys.ID_ENTER)
Thread.yield();
// wait until ESCAPE key is pressed, then wait for it // to be released
while (buttons.getButtons() != Keys.ID_ESCAPE)
Thread.yield();
// once escape is released, indicate that it was and
// pause for a moment
// (i.e. 5 seconds) before exiting
lcddisplay.drawString("Escape was pressed", 0, 1);
Stopwatch sw = new Stopwatch();
while (sw.elapsed() < 5000)
Thread.yield();
}
}
Navigator API 编程实践
leJOS Navigator 界面提供了一组方法，可用于精确控制机器人的运动，包括移动到任何位置和控制运动方向的方法。在这个练习环节中，你将编写一个程序，让机器人在二维平面内行走，如图 4-3 所示:
图 4-3。
A two-dimensional plane for testing navigator API
starting at A(0, 0),
go to B(50, 50),
then, go back A(0, 0)
then go to C(-50, 50),
then finally come back to A(0, 0).
Note
在本练习课中，坐标的单位是厘米。因此，你必须确保你的轨道宽度和直径设置为厘米。
该程序的伪代码如下:
DifferentialPilot ev3robot = new DifferentialPilot(diam,trackwidth,Motor.A,Motor.C);
NavPathController navbot = new NavPathController(ev3robot);
Button waitForPress to start
Display destination’s coordinate (50, 50) on LCD
Display the message "Press ENTER key" to continue; Go to location with coordinate 50,50
Display destination’s coordinate (0, 0) on LCD
Display the message "Press ENTER key" to continue; Go to location with coordinate 0,0
Display destination’s coordinate (-50, 50) on LCD Display the message "Press ENTER key" to continue; Go to location with coordinate -50,50
Display destination’s coordinate (0, 0) on LCD Display the message "Press ENTER key" to continue; Go to location with coordinate 0,0
Button waitForPress to exit
下面的示例程序ch4p7.java向您展示了如何完成这个练习。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch4p7.java
//A programming example to practice navigator API functions
//******************************************************************
// import EV3 hardware packages for EV brick finding, activating
// keys and LCD
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.TextLCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.MovePilot;
import lejos.robotics.navigation.Navigator;
import lejos.robotics.navigation.Waypoint;
import lejos.utility.TextMenu;
public class ch4p7 {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(MotorPort.C);
public static void main(String[] args) throws InterruptedException {
// menu options for displaying navigator controlling
String[] menuItems = new String[1];
menuItems[0] = "Task 1: Navigator";
// display menu
TextMenu menu = new TextMenu(menuItems);
menu.setTitle("NavRobot");
// setup the wheel diameter of left (and right) motor // in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center // of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot ev3robot = new MovePilot(chassis);
Navigator navbot = new Navigator(ev3robot);
// run routine based on menu choice
switch (menu.select()) {
case 0:
navigate(navbot);
break;
}
}
// navigate()
// demo navigating using way points
private static void navigate(Navigator nav) {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
TextLCD lcddisplay = ev3brick.getTextLCD();
// set up way points
Waypoint wp1 = new Waypoint(50, 50);
Waypoint wp2 = new Waypoint(0, 0);
Waypoint wp3 = new Waypoint(-50, 50);
// clear menu
lcddisplay.clear();
// display current location
lcddisplay.drawString("At 0, 0", 0, 0);
lcddisplay.drawString("Press ENTER", 0, 2);
lcddisplay.drawString("to continue", 0, 3);
// wait until ENTER key is pressed
while (buttons.waitForAnyPress() != Keys.ID_ENTER)
Thread.yield();
// navigate to way point one, display new location
lcddisplay.clear();
nav.goTo(wp1);
lcddisplay.drawString("At 50, 50", 0, 0);
lcddisplay.drawString("Press ENTER", 0, 2);
lcddisplay.drawString("to continue", 0, 3);
// wait until ENTER key is pressed
while (buttons.waitForAnyPress() != Keys.ID_ENTER)
Thread.yield();
// navigate to way point two, display new location
lcddisplay.clear();
nav.goTo(wp2);
lcddisplay.drawString("At 0, 0", 0, 0);
lcddisplay.drawString("Press ENTER", 0, 2);
lcddisplay.drawString("to continue", 0, 3);
// wait until ENTER key is pressed
while (buttons.waitForAnyPress() != Keys.ID_ENTER)
Thread.yield();
// navigate to way point 3, display new location
lcddisplay.clear();
nav.goTo(wp3);
lcddisplay.drawString("At -50, 50", 0, 0);
lcddisplay.drawString("Press ENTER", 0, 2);
lcddisplay.drawString("to continue", 0, 3);
// wait until ENTER key is pressed
while (buttons.waitForAnyPress() != Keys.ID_ENTER)
Thread.yield();
// navigate to way point 2, display new location
lcddisplay.clear();
nav.goTo(wp2);
lcddisplay.drawString("At 0, 0", 0, 0);
lcddisplay.drawString("Press ENTER", 0, 2);
lcddisplay.drawString("to exit", 0, 3);
// wait until ENTER key is pressed
while (buttons.waitForAnyPress() != Keys.ID_ENTER)
Thread.yield();
}
}
在图 4-3 所示的笛卡尔坐标系中已经看到，节点 A 的坐标为(0，0)，节点 B 的坐标为(50，50)，节点 C 的坐标为(-50，50)。机器人的路径从节点 A 开始，遍历到节点 B，然后回溯到节点 A，遍历到节点 C，最后回溯到节点 A。节点 A、B 和 C 实际上形成了二叉树中的小元素，并且该实践显示了遍历二叉树的最简单形式。
摘要
在这一章中，你学习了 EV3 使用的笛卡尔坐标系统。您还学习了如何在导航类中应用各种方法来控制轮式车辆，以便它们可以在二维平面中用坐标描绘出预定义的路径。此外，您还了解了 EV3 砖块的主要硬件组件:即 LCD 显示屏和 EV3 砖块上用于与机器人交互的六个按键。最后，您学习了应用内置系统方法来控制 LCD 显示器和按钮，以便向机器人提供输入和/或从机器人获得输出。
在下一章，你将进入本书的下一部分，也就是说，通过使用搜索策略为你的机器人建立智能。特别是，将学习如何在小型 Java 虚拟机(JVM)上实现深度优先搜索(DFS)算法，即 LeJOS EV3，然后将 DFS 算法集成到基于 LeJOS 的机器人系统中进行定位和路径规划，增强 LeJOS 系统中现有的寻路方法。
五、深度优先搜索算法及其在乐高 EV3 中的实现
搜索行为属于人工智能(AI)范畴。人工智能的主要目标是赋予计算机思考的能力:换句话说，就是模仿人类的行为。不幸的是，这种模仿的问题在于，计算机的“大脑”并不像人脑一样工作:计算机需要一系列合理的步骤来处理，以找到解决方案。因此，你的目标是解构一个复杂的任务，并将其转化为机器人系统可以处理的简单步骤。这种从复杂到简单的转换，就是搜索算法要做的。
在这一章中，你将学习如何在小型 Java 虚拟机(JVM)上实现深度优先搜索(DFS)算法:当然，这就是莱霍斯·EV3。众所周知，leJOS EV3 是一个开源项目，旨在开发一个技术基础设施，将 Java 技术应用于机器人编程软件。Java 是一种面向对象的编程语言。leJOS 中实现的最重要的功能之一是 leJOS 导航 API(在第四章中讨论)，它可以用来实现一个目标，其中提供了一组方便的类和方法来控制机器人。
控制车辆的类处理几个层次的抽象。在最底部，有转动轮子的马达，由RegulatedMotor级控制。MovePilot(或 leJOS NXJ 中的DifferentialPilot)类使用电机控制基本动作:原地旋转、直线行进或弧形行进。在下一个级别，NavPathController使用DifferentialPilot来移动机器人通过平面上的复杂路径。为了执行导航，路径控制器需要机器人的位置和它前进的方向。它使用一个OdometeryPoseProvider来保持这个信息是最新的。特别是，本章将涵盖以下主题:
- 一种新的深度优先搜索(DFS)算法，可用于构建任意树结构。
- 在基于 leJOS 的机器人系统中应用和集成提出的 DFS 算法用于定位和路径规划，在该过程中增强 leJOS 系统中现有的寻路方法。
DFS 算法概述
我们先来考察一下人类是如何解决搜索问题的。首先，需要描述搜索问题是如何存在的。图 5-1 是一个搜索树的例子。这是一系列相互连接的节点，我们将通过这些节点进行搜索:
图 5-1。
Tree structure of a path
深度优先搜索(DFS)的工作原理是取一个节点；检查它的邻居；扩展它在那些邻居中找到的第一个节点；检查展开的节点是否是您的目的地；如果没有，继续探索更多的节点。例如，如果你想找到一条从 A 到 E 的路径，你可以使用两个列表来跟踪你在做什么:一个开放列表和一个封闭列表。开放列表记录你需要做什么，封闭列表记录你已经做了什么。
开始的时候，你只有你的起点，节点 a，你还没对它做什么，那就把它加入你的开放列表吧。然后你有一个包含<A>的开放列表和一个包含<empty>的封闭列表。现在让我们探索一下你的 A 节点的邻居。节点 A 的邻居是 B、C 和 D 节点。因为您现在已经完成了您的 A 节点，所以您可以将它从您的开放列表中移除，并将其添加到您的封闭列表中。您当前的开放列表包含<B, C, D>，而封闭列表包含<A>。现在我们的开放列表包含三个项目。
对于深度优先搜索，您总是从开放列表中探索第一个项目。开放列表中的第一项是 B 节点。b 不是你的目的地，我们来探索一下它的邻居。因为您现在已经扩展了 B，所以您将把它从开放列表中移除，并将其添加到封闭列表中。您的新节点是 E、F 和 G，您将这些节点添加到开放列表的开头。然后你有一个包含<A, B>,的开放列表和一个包含<E, F, G, C, D>的封闭列表。现在展开 E 节点。既然是你预定的目的地，就应该停下来。因此，您会收到通过使用常规的深度优先搜索算法从封闭列表中解释的路由A->B->E。
接下来，让我们看看如何使用深度搜索方法来解决路径定位问题，该问题可以应用于从起始城市导航到目的城市的 GPS 系统。假设您想从城市 A(例如新罕布什尔州的基恩)开车到城市 S(比如马萨诸塞州的波士顿)。给定以下路线，使用深度优先搜索策略，确定一个从 A 开始到 S 结束的计划。
| 城市 | 距离 | | --- | --- | | 从 a 到 B | 20 英里 | | 从 a 到 C | 10 英里 | | 从 a 到 D | 10 英里 | | 从 a 到 E | 20 英里 | | （back to back）背对背 | 10 英里 | | b 到 M | 20 英里 | | b 到 G | 10 英里 | | c 到 H | 10 英里 | | c 到 I | 15 英里 | | 从 d 到 J | 20 英里 | | e 到 K | 15 英里 | | e 到 L | 15 英里 | | m 到 N | 20 英里 | | 摩托 g | 20 英里 | | 从 I 到 P | 40 英里 | | 个人对个人 | 20 英里 | | 个人对个人 | 20 英里 |基于以上信息，你可以绘制一个路径树，如图 5-2 所示，显示两个城市之间所有可能的路径:
图 5-2。
Routes in between two cities
以下程序是一个示例代码，可用于通过使用深度优先搜索算法为您自动安排旅行计划找到一条路径:
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch5p1_main.java
//Driver class to set up map using ch5p1_GraphNode, ch5p1_Link, and //ch5p1_Graph classes.
//Calls a depth-first search in ch5p1_Graph class to create //navigation path from a start
//and end node.
//******************************************************************
public class ch5p1_main {
public static void main(String[] args) {
// These objects used to define what your graph looks // like
ch5p1_Graph searchGraph = new ch5p1_Graph();
ch5p1_GraphNode A, B, C, D, E, F, G, H, I, J, K, L, M, N, O, P, R, S;
ch5p1_Link AB, AC, AD, AE, BF, BM, BG, CH, CI, DJ, EK, EL, MN, MO, IP, PR, PS;
// define each node
A = new ch5p1_GraphNode("A");
B = new ch5p1_GraphNode("B");
C = new ch5p1_GraphNode("C");
D = new ch5p1_GraphNode("D");
E = new ch5p1_GraphNode("E");
F = new ch5p1_GraphNode("F");
G = new ch5p1_GraphNode("G");
H = new ch5p1_GraphNode("H");
I = new ch5p1_GraphNode("I");
J = new ch5p1_GraphNode("J");
K = new ch5p1_GraphNode("K");
L = new ch5p1_GraphNode("L");
M = new ch5p1_GraphNode("M");
N = new ch5p1_GraphNode("N");
O = new ch5p1_GraphNode("O");
P = new ch5p1_GraphNode("P");
R = new ch5p1_GraphNode("R");
S = new ch5p1_GraphNode("S");
// define which GraphNodes are connected
AB = new ch5p1_Link(A, B);
AC = new ch5p1_Link(A, C);
AD = new ch5p1_Link(A, D);
AE = new ch5p1_Link(A, E);
BF = new ch5p1_Link(B, F);
BM = new ch5p1_Link(B, M);
BG = new ch5p1_Link(B, G);
CH = new ch5p1_Link(C, H);
CI = new ch5p1_Link(C, I);
DJ = new ch5p1_Link(D, J);
EK = new ch5p1_Link(E, K);
EL = new ch5p1_Link(E, L);
MN = new ch5p1_Link(M, N);
MO = new ch5p1_Link(M, O);
IP = new ch5p1_Link(I, P);
PR = new ch5p1_Link(P, R);
PS = new ch5p1_Link(P, S);
// add all nodes and links to your graph object
searchGraph.addNode(A);
searchGraph.addNode(B);
searchGraph.addNode(C);
searchGraph.addNode(D);
searchGraph.addNode(E);
searchGraph.addNode(F);
searchGraph.addNode(G);
searchGraph.addNode(H);
searchGraph.addNode(I);
searchGraph.addNode(J);
searchGraph.addNode(K);
searchGraph.addNode(L);
searchGraph.addNode(M);
searchGraph.addNode(N);
searchGraph.addNode(O);
searchGraph.addNode(P);
searchGraph.addNode(R);
searchGraph.addNode(S);
searchGraph.addLink(AB);
searchGraph.addLink(AC);
searchGraph.addLink(AD);
searchGraph.addLink(AE);
searchGraph.addLink(BF);
searchGraph.addLink(BM);
searchGraph.addLink(BG);
searchGraph.addLink(CH);
searchGraph.addLink(CI);
searchGraph.addLink(DJ);
searchGraph.addLink(EK);
searchGraph.addLink(EL);
searchGraph.addLink(MN);
searchGraph.addLink(MO);
searchGraph.addLink(IP);
searchGraph.addLink(PR);
searchGraph.addLink(PS);
// run depth-first search to get from start to
// destination
searchGraph.dfsTraverse(
searchGraph.nodes.get(searchGraph.nodes.indexOf(A)),
searchGraph.nodes.get(searchGraph.nodes.indexOf(S)));
// display path created using dfsTraverse
// This will be display the path from start to
// destination
System.out.println("the path from your current city to the destination city is: ");
for (int i = searchGraph.dfsPath.size() - 1; i >= 0; i--) {
if(i!=0)
System.out.print(searchGraph.dfsPath.get(i).cityName + "->");
else System.out.print(searchGraph.dfsPath.get(i).cityName);
}
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3
//ch5p1_GraphNode.java
//Represents name of a graph
//******************************************************************
public class ch5p1_GraphNode {
String cityName;
public ch5p1_GraphNode(String cityName) {
this.cityName = cityName;
}
public String toString() {
return cityName;
}
}
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch5p1_Graph.java
//Represents GraphNodes connected through Links, including method //for doing a depth-first
//search traversal of the graph.
//the method dfsTraverse creates a dfsPath list which the robot will //follow to demonstrate
//how the depth-first search works.
//******************************************************************************************
import java.util.ArrayList;
import java.util.Stack;
public class ch5p1_Graph {
// nodes and links define the physical creation of your Graph
ArrayList<ch5p1_GraphNode> nodes;
ArrayList<ch5p1_Link> links;
// Two lists used for traversing
ArrayList<ch5p1_GraphNode> dfsTraverse;
Stack<ch5p1_Link> travelStack = new Stack<ch5p1_Link>();
// List used to define where you would like to move
ArrayList<ch5p1_GraphNode> dfsPath = new ArrayList<ch5p1_GraphNode>();
// Constructor of ch5p1_Graph class
public ch5p1_Graph() {
nodes = new ArrayList<ch5p1_GraphNode>();
links = new ArrayList<ch5p1_Link>();
dfsTraverse = new ArrayList<ch5p1_GraphNode>();
}// end constructor
// addNode()
// Add a city node to the graph
public void addNode(ch5p1_GraphNode node) {
nodes.add(node);
}// end addNode
// addLink
// Add link to the graph
public void addLink(ch5p1_Link link) {
links.add(link);
}// end addLink
// dfsTraverse()
// perform depth-first search on the graph
public void dfsTraverse(ch5p1_GraphNode from, ch5p1_GraphNode to) {
boolean matched;
ch5p1_Link found;
// determine if there is a link between from and to
// if there is a match then add the link to the
// travelStack and
// add the nodes to dfsPath
// This will ultimately repeated by the end of the
// search
matched = match(from, to);
if (matched) {
travelStack.push(new ch5p1_Link(from, to));
dfsPath.add(new ch5p1_GraphNode(to.cityName));
dfsPath.add(new ch5p1_GraphNode(from.cityName));
return;
}
// if there is no match found you could another path
// findings
found = find(from);
// if you find a new connection then you could add it // to the travelStack
// and
// and the start node to dfsPath
// recursively call dfsTraverse with the link's to as // start and our
// destination as the end
if (found != null) {
travelStack.push(new ch5p1_Link(from, to));
dfsTraverse(found.to, to);
dfsPath.add(new ch5p1_GraphNode(from.cityName));
}
// backtrack if you cannot find a new connection
else if (travelStack.size() > 0) {
found = travelStack.pop();
dfsTraverse(found.from, found.to);
dfsPath.add(new ch5p1_GraphNode(from.cityName));
}
}// end dfsTraverse()
// find() method is used to
// find the next link to try exploring
public ch5p1_Link find(ch5p1_GraphNode from) {
// iterate through the list of links
for (int a = 0; a < links.size(); a++) {
// link found
if (links.get(a).from.equals(from) && !links.get(a).skip) {
ch5p1_Link foundList = new ch5p1_Link(links.get(a).from,
links.get(a).to);
// mark this link as used so we don't
// match it again
links.get(a).skip = true;
return foundList;
}
}
return null; // not found
}// end find()
// match() method is used to determine if there is a link
// between a starting
// node and an ending node
public boolean match(ch5p1_GraphNode from, ch5p1_GraphNode to) {
// iterate through list of links
for (int a = links.size() - 1; a >= 0; a--) {
if (links.get(a).from.equals(from) && links.get(a).to.equals(to)
&& !links.get(a).skip) {
links.get(a).skip = true;
return true;
}
}
return false;
}// end match()
}// end Graph.java
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch5p1_Link.java
//Represents a link between two GraphNodes
//******************************************************************
public class ch5p1_Link {
ch5p1_GraphNode from;
ch5p1_GraphNode to;
// boolean skip is used for traversal to determine if the path // has already
// been visited or not
boolean skip;
public ch5p1_Link(ch5p1_GraphNode from, ch5p1_GraphNode to) {
this.from = from;
this.to = to;
skip = false;
}
}
在上面的程序中，dfsTraverse()方法应用了一种递归方法来执行深度优先搜索，其中使用了一种match()方法来检查线路图并确定在 from 和 to 之间是否有任何路径。如果答案是肯定的，那么它获得信息并将路由连接信息推入栈，然后返回路由路径。否则，如果答案是否定的，它就调用一个find(方法来检查 from 和其他城市之间的路径，如果可以找到，程序就返回连接信息的对象，否则程序返回 null。如果它能找到这样的路径，那么它可以将信息推到栈顶，然后递归地调用dfsTraverse()，并将城市保存到新的出发城市中。否则，程序返回并递归调用dfsTraverse()，直到找到目的地。
运行该程序的结果如下:
The path from your current city to the destination city is:
A->B->F->B->M->N->M->O->M->B->G->B->A->C->H->C->I->P->S
这个结果显示了使用 DFS 搜索策略遍历图时的确切路径，包括更详细的回溯。
基于莱霍斯·EV3 的 DFS 算法
在基于 leJOS 的 DFS 算法中，路径上的每个节点都是一个名为 WPNode 的类节点，定义如下:
public WPNode(String newname, WayPoint newwp) {
nodename = newname;
nodewp = newwp;
seen = false;
parent = this;
connections = new ArrayList<WPNode>();
}
基于 leJOS 的 DFS 算法的伪代码描述如下:
-
构建搜索空间的类属树，如:
A = new WPNode("A", new WayPoint(0, 0)); B = new WPNode("B", new WayPoint(-5, 5)); C = new WPNode("C", new WayPoint(5, 5)); A.addLeaf(B); A.addLeaf(C); -
声明一个栈来保存路由路径，例如:
Stack<WPNode> DFSpath = new Stack<WPNode>(); -
将当前节点设置为根节点，比如 a。如果找不到目标节点，则循环以下内容:
- 如果当前节点有子节点，则将第一个不可见的节点设置为当前节点，然后返回。
- 如果当前节点没有看不见的子节点，则将其父节点设置为当前节点，然后返回。
-
找到目的节点后，将目的节点推入栈，然后将每个父节点推入栈。
-
为双马达运动生成引导，然后设置引导以使用适当的尺寸和马达。
-
弹出每个路径节点的航路点，应用
goto(int x, int y)方法指挥机器人移动到下一个节点。
基于上述基于 leJOS 的 DFS 算法，你可以为你的机器人开发一个程序，使它能够在起始节点 A 和目的节点 M 之间的路径上行进，如图 5-3 所示:
- A 处的坐标是(0，0)。
- B 点的坐标是(-5，5)。
- C 点的坐标是(5，5)。
- D 点的坐标是(-10，10)。
- E 点的坐标是(0，10)。
- F 点的坐标是(-5，15)。
- 在 G 点的坐标是(5，15)。
- H 处的坐标是(-10，20)。
- I 处的坐标是(0，20)。
- J 点的坐标是(-15，25)。
- K 处的坐标是(-5，25)。
- L 处的坐标是(-10，30)。
- M 处的坐标为(0，30)。
图 5-3。
Path in between two nodes
你的程序至少应该在 LCD 上显示目的地的坐标，然后显示消息“Press ENTER key to continue.”，按回车键，你的机器人移动到下一个节点。例如，假设你的机器人从A (0,0)出发，你想探索一条通往E (0,10)的道路。假设你的机器人使用深度优先搜索探索的路径是A -> B -> E。在起点 A，您的程序应该执行以下操作:
- 在 LCD 上显示目的地的坐标
B(-5,5)，并显示信息Press ENTER key to continue。 - 转到坐标为
-5,5的位置。 - 在 LCD 上显示目的地的坐标
E(0, 10)，并显示信息Press ENTER key to continue. - 转到坐标为
0,10的位置。
而且你的问题应该有一个字符串叫 destination，所以在改变 destination 的值的时候足够智能，你的机器人可以探索一条从起始节点 A 到新的目的节点的新路径。(我们假设起始节点总是 A，所以 from 字符串可以硬编码。)
以下程序表示基于 leJOS 的 DFS 算法的实现，以探索从节点 A 到目的节点 M 的路径:
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch5p2_main.java
//Driver class to set up map using ch5p2_GraphNode, ch5p2_Link, and //ch5p2_Graph classes.
//Calls a depth-first search in ch5p2_Graph class to create //navigation path from a start
//and end node and then robots will follow the path to move from //start node
//to destination node
//******************************************************************
// import EV3 hardware packages for EV brick finding, activating
// keys and LCD
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.MovePilot;
import lejos.robotics.navigation.Navigator;
public class ch5p2_main {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(
MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(
MotorPort.C);
public static void main(String[] args) {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
// setup the wheel diameter of left (and right) motor // in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center // of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot ev3robot = new MovePilot(chassis);
Navigator navbot = new Navigator(ev3robot);
// These objects used to define what your graph looks // like
ch5p2_Graph searchGraph = new ch5p2_Graph();
ch5p2_GraphNode A, B, C, D, E, F, G, H, I, J, K, L, M;
ch5p2_Link AB, AC, BD, BE, EF, EG, FH, FI, HJ, HK, KL, KM;
// define each node
A = new ch5p2_GraphNode("A", 0, 0);
B = new ch5p2_GraphNode("B", -5, 5);
C = new ch5p2_GraphNode("C", 5, 5);
D = new ch5p2_GraphNode("D", -10, 10);
E = new ch5p2_GraphNode("E", 0, 10);
F = new ch5p2_GraphNode("F", -5, 15);
G = new ch5p2_GraphNode("G", 5, 15);
H = new ch5p2_GraphNode("H", -10, 20);
I = new ch5p2_GraphNode("I", 0, 20);
J = new ch5p2_GraphNode("J", -15, 25);
K = new ch5p2_GraphNode("K", -5, 25);
L = new ch5p2_GraphNode("L", -10, 30);
M = new ch5p2_GraphNode("M", 0, 30);
// define which GraphNodes are connected
AB = new ch5p2_Link(A, B);
AC = new ch5p2_Link(A, C);
BD = new ch5p2_Link(B, D);
BE = new ch5p2_Link(B, E);
EF = new ch5p2_Link(E, F);
EG = new ch5p2_Link(E, G);
FH = new ch5p2_Link(F, H);
FI = new ch5p2_Link(F, I);
HJ = new ch5p2_Link(H, J);
HK = new ch5p2_Link(H, K);
KL = new ch5p2_Link(K, L);
KM = new ch5p2_Link(K, M);
// add all nodes and links to your graph object
searchGraph.addNode(A);
searchGraph.addNode(B);
searchGraph.addNode(C);
searchGraph.addNode(D);
searchGraph.addNode(E);
searchGraph.addNode(F);
searchGraph.addNode(G);
searchGraph.addNode(H);
searchGraph.addNode(I);
searchGraph.addNode(J);
searchGraph.addNode(K);
searchGraph.addNode(L);
searchGraph.addNode(M);
searchGraph.addLink(AB);
searchGraph.addLink(AC);
searchGraph.addLink(BD);
searchGraph.addLink(BE);
searchGraph.addLink(EF);
searchGraph.addLink(EG);
searchGraph.addLink(FH);
searchGraph.addLink(FI);
searchGraph.addLink(HJ);
searchGraph.addLink(HK);
searchGraph.addLink(KL);
searchGraph.addLink(KM);
// block the thread until a button is pressed
buttons.waitForAnyPress();
// run depth-first search to get from start to
// destination
searchGraph.dfsTraverse(
searchGraph.nodes.get(searchGraph.nodes.indexOf(A)),
searchGraph.nodes.get(searchGraph.nodes.indexOf(M)));
// block the thread until a button is pressed
buttons.waitForAnyPress();
// Robot moves through path from start to destination // by using
// dfsTraverse
for (int i = searchGraph.dfsPath.size() - 1; i >= 0; i--) {
// go to node
navbot.goTo(searchGraph.dfsPath.get(i).xLocation,
searchGraph.dfsPath.get(i).yLocation);
LCD.clear();
// display current location
LCD.drawString("At location " + searchGraph.dfsPath.get(i).cityName
+ ", ", 0, 0);
LCD.drawString(searchGraph.dfsPath.get(i).xLocation + ", "
+ searchGraph.dfsPath.get(i).yLocation, 0, 1);
LCD.drawString("Press ENTER key", 0, 2);
// block the thread until a button is pressed
buttons.waitForAnyPress();
}
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3
// ch5p2_GraphNode.java
//Represents name and coordinates of a node on a graph
//******************************************************************
public class ch5p2_GraphNode {
String cityName;
int xLocation, yLocation;
public ch5p2_GraphNode(String cityName, int xLocation, int yLocation) {
this.cityName = cityName;
this.xLocation = xLocation;
this.yLocation = yLocation;
}
public String toString() {
return cityName + " ("+xLocation +"," + yLocation + ")";
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch5p2_Link.java
//Represents a link between two GraphNodes
//******************************************************************
public class ch5p2_Link {
ch5p2_GraphNode from;
ch5p2_GraphNode to;
// boolean skip is used for traversal to determine if the path // has already
// been visited or not
boolean skip;
public ch5p2_Link(ch5p2_GraphNode from, ch5p2_GraphNode to) {
this.from = from;
this.to = to;
skip = false;
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch5p2_Graph.java
//Represents GraphNodes connected through Links, including method //for doing a depth-first
//search of the graph.
//the method dfsTraverse creates a dfsPath list which the robot will //follow to demonstrate
//how the depth-first search works.
//******************************************************************
import java.util.ArrayList;
import java.util.Stack;
public class ch5p2_Graph {
// nodes and links define the physical creation of your Graph
ArrayList<ch5p2_GraphNode> nodes;
ArrayList<ch5p2_Link> links;
// Two lists used for traversing
ArrayList<ch5p2_GraphNode> dfsTraverse;
Stack<ch5p2_Link> travelStack = new Stack<ch5p2_Link>();
// List used to define where you would like to move
ArrayList<ch5p2_GraphNode> dfsPath = new ArrayList<ch5p2_GraphNode>();
// Constructor of ch5p2_Graph class
public ch5p2_Graph() {
nodes = new ArrayList<ch5p2_GraphNode>();
links = new ArrayList<ch5p2_Link>();
dfsTraverse = new ArrayList<ch5p2_GraphNode>();
}// end constructor
// addNode()
// Add a city node to the graph
public void addNode(ch5p2_GraphNode node) {
nodes.add(node);
}// end addNode
// addLink
// Add link to the graph
public void addLink(ch5p2_Link link) {
links.add(link);
}// end addLink
// dfsTraverse()
// perform depth-first search on the graph
public void dfsTraverse(ch5p2_GraphNode from, ch5p2_GraphNode to) {
boolean matched;
ch5p2_Link found;
// determine if there is a link between from and to
// if there is a match then add the link to the
// travelStack and
// add the nodes to dfsPath
// This will be ultimately repeated by the end of the
// search
matched = match(from, to);
if (matched) {
travelStack.push(new ch5p2_Link(from, to));
dfsPath.add(new ch5p2_GraphNode(to.cityName,to.xLocation, to.yLocation));
dfsPath.add(new ch5p2_GraphNode(from.cityName,from.xLocation, from.yLocation));
return;
}
// if there is no match found you could another path
// findings
found = find(from);
// if you find a new connection then you could add it // to the travelStack
// and
// and the start node to dfsPath
// recursively call dfsTraverse with the link's to as // start and our
// destination as the end
if (found != null) {
travelStack.push(new ch5p2_Link(from, to));
dfsTraverse(found.to, to);
dfsPath.add(new ch5p2_GraphNode(from.cityName,from.xLocation, from.yLocation));
}
// backtrack if you cannot find a new connection
else if (travelStack.size() > 0) {
found = travelStack.pop();
dfsTraverse(found.from, found.to);
dfsPath.add(new ch5p2_GraphNode(from.cityName,from.xLocation, from.yLocation));
}
}// end dfsTraverse()
// find() method is used to
// find the next link to try exploring
public ch5p2_Link find(ch5p2_GraphNode from) {
// iterate through the list of links
for (int a = 0; a < links.size(); a++) {
// link found
if (links.get(a).from.equals(from) && !links.get(a).skip) {
ch5p2_Link foundList = new ch5p2_Link(links.get(a).from,
links.get(a).to);
// mark this link as used so we don't match it again
links.get(a).skip = true;
return foundList;
}
}
return null; // not found
}// end find()
// match() method is used to determine if there is a link
// between a starting
// node and an ending node
public boolean match(ch5p2_GraphNode from, ch5p2_GraphNode to) {
// iterate through list of links
for (int a = links.size() - 1; a >= 0; a--) {
if (links.get(a).from.equals(from) && links.get(a).to.equals(to)
&& !links.get(a).skip) {
links.get(a).skip = true;
return true;
}
}
return false;
}// end match()
}// end Graph.java
摘要
在这一章中，你学习了深度优先搜索算法的基本原理，现在知道如何在实践中应用它来解决搜索程序。您还学习了如何基于深度优先搜索算法和导航类构建问题解决代理(即您的机器人)，您在前面的章节中学习了这些问题解决代理能够智能地找到从起点到任何目的地的路线路径。
在下一章中，你将学习如何在 leJOS EV3 上实现广度优先搜索(BFS)算法，以及如何将已实现的 BFS 算法集成到基于 leJOS 的机器人系统中进行定位和路径规划。
六、广度优先搜索及其在 Lego Mindstorms 中的实现
正如你在第五章中被介绍到深度优先搜索(DFS)算法一样，在这一章你将学习如何在莱霍斯 EV3 实现广度优先搜索(BFS)算法。具体而言，本章将涵盖以下主题:
- 一种新的广度优先搜索(BFS)算法，可以应用于建立任意树结构一般。
- 在基于 leJOS 的机器人系统中应用和集成所提出的 BFS 算法用于定位和路径规划，这增强了 leJOS 系统中现有的寻路方法。
BFS 算法综述
我们先来回顾一下人类是如何解决一个搜索问题的。首先，你需要一个表示你的搜索问题是如何存在的。图 6-1 是你的搜索树的一个例子。它显示了一系列相互连接的节点，您可以通过这些节点进行搜索:
图 6-1。
Tree structure of a path
广度优先搜索算法的工作原理是取一个节点；检查它的邻居；扩展它在其邻居中找到的第一个节点；检查该扩展节点是否是其目的地；如果没有，则继续探索其他相邻节点。例如，如果你想找到一条从 A 到 E 的路径，你可以使用两个列表来跟踪你在做什么:一个开放列表和一个封闭列表。开放列表记录你需要做什么，封闭列表记录你已经做了什么。
开始的时候，你只有你的起点，节点 a，你还没对它做什么，那就把它加入你的开放列表吧。现在你有了一个包含<A>的开放列表和一个包含<empty>的封闭列表。
现在让我们探索一下你的 A 节点的邻居。节点 A 的邻居是 B、C 和 D 节点。因为您现在已经完成了 A 节点，所以您可以将它从开放列表中移除，并将其添加到封闭列表中。那么当前开放列表包含<B, C, D>,，封闭列表包含<A>。现在，您的开放列表包含三个项目。
对于广度优先搜索，您总是从开放列表中探索第一个节点。开放列表中的第一项是 B 节点。b 不是你的目的地，现在我们来探索一下它的邻居。因为您现在已经扩展了 B，所以您将把它从开放列表中移除，并将其添加到封闭列表中。您的新节点是 E、F 和 G，您可以将这些节点添加到开放列表的末尾。然后你有一个包含<A,B>的封闭列表和一个包含< C, D, E, F, G>的开放列表。
接下来，展开 C 节点。由于它不是预期的目的地，您可以将其从开放列表中删除，并将其添加到封闭列表中。当前开放列表包含<D, E, F, G>,，而封闭列表包含<A, B, C>。c 没有子节点，所以您继续访问节点 d。因为它也不是预期的目的地，所以您可以将其从开放列表中删除，并将其添加到封闭列表中。因此，当前开放列表包含<E, F, G>，而 and 关闭列表包含<A, B, C, D>。
接下来展开节点 e。因为这是您的预期目的地，所以您停下来。因此，您会收到通过使用常规的广度优先搜索算法从封闭列表中解释的路线A->B->E。
接下来，让我们看看如何使用广度优先搜索方法来解决路径查找问题，该问题可以应用于从起始城市导航到目的城市的 GPS 系统。假设您想从城市 A(例如，新罕布什尔州的基恩)开车到城市 S(例如，马萨诸塞州的波士顿)。给定以下路线，使用广度优先搜索策略，为你制定一个从 A 出发到达 S 的计划。
| 城市 | 距离 | | --- | --- | | 从 a 到 B | 20 英里 | | 从 a 到 C | 10 英里 | | 从 a 到 D | 10 英里 | | 从 a 到 E | 20 英里 | | （back to back）背对背 | 10 英里 | | b 到 M | 20 英里 | | b 到 G | 10 英里 | | c 到 H | 10 英里 | | c 到 I | 15 英里 | | 从 d 到 J | 20 英里 | | e 到 K | 15 英里 | | e 到 L | 15 英里 | | m 到 N | 20 英里 | | 摩托 g | 20 英里 | | 从 I 到 P | 40 英里 | | 个人对个人 | 20 英里 | | 个人对个人 | 20 英里 |基于以上信息，你可以绘制一个路径树，如图 6-2 所示，显示两个城市之间所有可能的路径:
图 6-2。
Routes in between two cities
以下程序可用于通过使用广度优先搜索算法为您自动安排旅行计划找到一条路径:
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch6p1_main.java
//Driver class to call a breadth-first search algorithm
//to create navigation path from a start to an end node.
//******************************************************************
import java.util.*;
class ch6p1_main {
// This array holds the connection information between two cities.
ch6p1_GraphNode cityLinks[] = new ch6p1_GraphNode[200];
// number of path connections on the route graph
int numConnections = 0;
// backtrack stack to store the city nodes
Stack closedList = new Stack();
public static void main(String args[]) {
// define the start city name
String from = "A";
// define the destination city name
String to = "S";
ch6p1_main bfsTraveral = new ch6p1_main();
// set up the route graph
bfsTraveral.graph();
bfsTraveral.isNode(from, to);
if (bfsTraveral.closedList.size() != 0) {
System.out
.println("the path from your current city to the destination city is: ");
bfsTraveral.BFSroute(to);
}
}
// Add link between two cities
void addLink(String parent, String child) {
if (numConnections < 200) {
cityLinks[numConnections] = new ch6p1_GraphNode(parent, child);
numConnections++;
} else
System.out.println("error to add link\n");
}
// Initialize the path link to construct the graph.
void graph() {
addLink("A", "B");
addLink("A", "C");
addLink("A", "D");
addLink("A", "E");
addLink("B", "F");
addLink("B", "M");
addLink("B", "G");
addLink("C", "H");
addLink("C", "I");
addLink("D", "J");
addLink("E", "K");
addLink("E", "L");
addLink("M", "N");
addLink("M", "O");
addLink("I", "P");
addLink("P", "R");
addLink("P", "S");
}
// determine to see if there is a link matched between
// startCity and endCity
// if match found, return true, otherwise return false
int matched(String from, String to) {
for (int i = numConnections - 1; i > -1; i--) {
if (cityLinks[i].startCity.equals(from)
&& cityLinks[i].endCity.equals(to) && !cityLinks[i].visited) {
// set up visited to true to prevent re-visit
cityLinks[i].visited = true;
// match found
return 1;
}
}
// match not found
return 0;
}
// Given parent to find any child connected with this parent
ch6p1_GraphNode find(String parent) {
for (int i = 0; i < numConnections; i++) {
if (cityLinks[i].startCity.equals(parent) && !cityLinks[i].visited) {
ch6p1_GraphNode f = new ch6p1_GraphNode(cityLinks[i].startCity,
cityLinks[i].endCity);
// set up visited to true to prevent re-visit
cityLinks[i].visited = true;
// child (or leaf) returned
return f;
}
}
// if parent has no child return nothing
return null;
}
// using breadth-first search and determining if there is any // route existing
// in between startCity and endCity
void isNode(String from, String to) {
int directconn;
ch6p1_GraphNode citynode;
Stack resetList = new Stack();
// determine if there is any direct link between from and to
// if yes push the link of the two cities into closedList
// stack
directconn = matched(from, to);
if (directconn != 0) {
closedList.push(
new ch6p1_GraphNode(from, to));
return;
}
// find all the children cities connected with the
// specified parent node
while ((citynode = find(from)) != null) {
resetList.push(citynode);
// check further if there is any direct
// connection between the child
// and grandchild
if ((directconn = matched(citynode.endCity, to)) != 0) {
resetList.push(citynode.endCity);
closedList.push(
new ch6p1_GraphNode(from, citynode.endCity));
closedList.push(
new ch6p1_GraphNode(citynode.endCity, to));
return;
}
}
// reset the visited boolean to unvisited and do the
// breadth first
// search next
for (int i = resetList.size(); i != 0; i--)
resetSkip((ch6p1_GraphNode) resetList.pop());
// then try the next neighboring city nodes
citynode = find(from);
if (citynode != null) {
closedList.push(
new ch6p1_GraphNode(from, to));
isNode(citynode.endCity, to);
} else if (closedList.size() > 0) {
// trace back and try another link
citynode = (ch6p1_GraphNode) closedList.pop();
isNode(citynode.startCity, citynode.endCity);
}
}
// reset visited field of specified parent city node
void resetSkip(ch6p1_GraphNode citynode) {
for (int i = 0; i < numConnections; i++)
if (cityLinks[i].startCity.equals(citynode.startCity)
&& cityLinks[i].endCity.equals(citynode.endCity))
cityLinks[i].visited = false;
}
// Show the route obtained by the BFS algorithm
void BFSroute(String to) {
int num = closedList.size();
Stack reverseList = new Stack();
ch6p1_GraphNode citynode;
// Reverse the stack to show the path
for (int i = 0; i < num; i++)
reverseList.push(closedList.pop());
for (int i = 0; i < num; i++) {
citynode = (ch6p1_GraphNode) reverseList.pop();
System.out.print(citynode.startCity + " -> ");
}
System.out.println(to);
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3
//ch6p1_GraphNode.java
//Represents name of a graph
//******************************************************************
public class ch6p1_GraphNode {
String startCity;
String endCity;
// to determine if the city has been visited or not
boolean visited;
ch6p1_GraphNode(String s, String d) {
startCity = s;
endCity = d;
visited = false;
}
}
给定startCity A 和endCity S，运行程序的结果如下:
The path from your current city to the destination city is this:
A -> C -> I -> P -> S
这个结果显示了使用 BFS 搜索策略遍历图时的准确路径，不包括中间回溯节点。
基于莱霍斯·EV3 的 BFS 算法
在基于 leJOS 的 BFS 算法中，路径上的每个节点都是一个名为WPNode的类节点，定义如下:
public WPNode(String newname, WayPoint newwp) {
nodename = newname;
nodewp = newwp;
seen = false;
parent = this;
connections = new ArrayList<WPNode>();
}
以下是基于 leJOS 的 BFS 算法的伪代码描述:
-
构建搜索空间的类属树，如:
A = new WPNode("A", new WayPoint(0, 0)); B = new WPNode("B", new WayPoint(-5, 5)); C = new WPNode("C", new WayPoint(5, 5)); A.addLeaf(B); A.addLeaf(C); -
声明一个栈来保存路由路径，比如:
Stack<WPNode> BFSpath = new Stack<WPNode>(); -
将当前节点设置为根节点，比如 a。如果找不到目标节点，则循环以下内容:
- 如果当前节点有子节点，则将第一个不可见的节点设置为当前节点，然后返回。
- 如果当前节点没有看不见的子节点，则将其父节点设置为当前节点，然后返回。
-
找到目的节点后，将目的节点推入栈，然后将每个父节点推入栈。
-
为双马达运动生成引导，然后设置引导以使用适当的尺寸和马达。
-
弹出每个路径节点的路点，并应用
goto(int x, int y)方法来引导机器人移动到下一个节点。
基于上述基于 leJOS 的 BFS 算法，您可以为您的机器人开发一个程序，使其能够在起始节点 A 和目的节点 M 之间的路径上行进，如图 6-3 所示，其中您有以下内容:
图 6-3。
Path in between two nodes
The coordinate at A is (0,0).
The coordinate at B is (-5,5).
The coordinate at C is (5, 5).
The coordinate at D is (-10,10).
The coordinate at E is (0,10).
The coordinate at F is (-5,15).
The coordinate at G is (5,15).
The coordinate at H is (-10,20).
The coordinate at I is (0,20).
The coordinate at J is (-15,25).
The coordinate at K is (-5,25).
The coordinate at L is (-10,30).
The coordinate at M is (0,30).
你的程序至少应该在 LCD 上显示目的地的坐标，然后显示一条信息“Press ENTER key to continue”按回车键，你的机器人移动到下一个节点。例如，假设你的机器人从A (0,0)出发，你想探索一条通往G (5,15)的道路。假设你的机器人使用广度优先搜索探索的路径是A -> B -> E -> G。在起点 A，您的程序应该执行以下操作:
- 在 LCD 上显示目的地的坐标
B(-5, 5)，并显示信息Press ENTER key to continue - 转到坐标为
-5,5的位置。 - 在 LCD 上显示目的地的坐标
E(0, 10)，并显示信息Press ENTER key to continue - 转到坐标为
0,10的位置。 - 在 LCD 上显示目的地的坐标
G(5, 15)，并显示信息Press ENTER key to continue - 转到坐标为
5,15.的位置
此外，你的问题应该有一个名为destination的字符串，这样它就足够智能，当改变目的地的值时，你的机器人可以探索一条从起始节点 A 到新目的地节点的新路径。(我们假设起始节点总是 A，所以 from 字符串可以硬编码。)
以下程序表示基于 leJOS 的 BFS 算法的实现，以探索从节点 A 到目的节点 M 的路径:
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch6p2_main.java
//Driver class to set up map using ch6p2_GraphNode, ch6p2_Link, and //ch6p2_Graph classes.
//Calls a breadth-first search in ch6p2_Graph class to create //navigation path from a start
//and end node and then robots will follow the path to move from //start node
//to destination node
//******************************************************************************************
// import EV3 hardware packages for EV brick finding, activating
// keys and LCD
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.MovePilot;
import lejos.robotics.navigation.Navigator;
public class ch6p2_main {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(
MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(
MotorPort.C);
public static void main(String[] args) {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
// setup the wheel diameter of left (and right) motor // in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center // of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot ev3robot = new MovePilot(chassis);
Navigator navbot = new Navigator(ev3robot);
// These objects used to define what your graph looks like
ch6p2_Graph searchGraph = new ch6p2_Graph();
ch6p2_GraphNode A, B, C, D, E, F, G, H, I, J, K, L, M;
ch6p2_Link AB, AC, BD, BE, EF, EG, FH, FI, HJ, HK, KL, KM;
// define each node
A = new ch6p2_GraphNode("A", 0, 0);
B = new ch6p2_GraphNode("B", -5, 5);
C = new ch6p2_GraphNode("C", 5, 5);
D = new ch6p2_GraphNode("D", -10, 10);
E = new ch6p2_GraphNode("E", 0, 10);
F = new ch6p2_GraphNode("F", -5, 15);
G = new ch6p2_GraphNode("G", 5, 15);
H = new ch6p2_GraphNode("H", -10, 20);
I = new ch6p2_GraphNode("I", 0, 20);
J = new ch6p2_GraphNode("J", -15, 25);
K = new ch6p2_GraphNode("K", -5, 25);
L = new ch6p2_GraphNode("L", -10, 30);
M = new ch6p2_GraphNode("M", 0, 30);
// define which GraphNodes are connected
AB = new ch6p2_Link(A, B);
AC = new ch6p2_Link(A, C);
BD = new ch6p2_Link(B, D);
BE = new ch6p2_Link(B, E);
EF = new ch6p2_Link(E, F);
EG = new ch6p2_Link(E, G);
FH = new ch6p2_Link(F, H);
FI = new ch6p2_Link(F, I);
HJ = new ch6p2_Link(H, J);
HK = new ch6p2_Link(H, K);
KL = new ch6p2_Link(K, L);
KM = new ch6p2_Link(K, M);
// add all nodes and links to your graph object
searchGraph.addNode(A);
searchGraph.addNode(B);
searchGraph.addNode(C);
searchGraph.addNode(D);
searchGraph.addNode(E);
searchGraph.addNode(F);
searchGraph.addNode(G);
searchGraph.addNode(H);
searchGraph.addNode(I);
searchGraph.addNode(J);
searchGraph.addNode(K);
searchGraph.addNode(L);
searchGraph.addNode(M);
searchGraph.addLink(AB);
searchGraph.addLink(AC);
searchGraph.addLink(BD);
searchGraph.addLink(BE);
searchGraph.addLink(EF);
searchGraph.addLink(EG);
searchGraph.addLink(FH);
searchGraph.addLink(FI);
searchGraph.addLink(HJ);
searchGraph.addLink(HK);
searchGraph.addLink(KL);
searchGraph.addLink(KM);
// block the thread until a button is pressed
buttons.waitForAnyPress();
// run breadth-first search to get from start to
// destination
searchGraph.bfsTraverse(
searchGraph.nodes.get(searchGraph.nodes.indexOf(A)),
searchGraph.nodes.get(searchGraph.nodes.indexOf(M)));
// block the thread until a button is pressed
buttons.waitForAnyPress();
// Robot moves through path from start to destination // by using
// bfsTraverse
for (int i = 0; i < searchGraph.bfsTraverse.size(); i++) {
// go to node
navbot.goTo(searchGraph.bfsPath.get(i).xLocation,
searchGraph.bfsPath.get(i).yLocation);
LCD.clear();
// display current location
LCD.drawString("At location " + searchGraph.bfsPath.get(i).cityName
+ ", ", 0, 0);
LCD.drawString(searchGraph.bfsPath.get(i).xLocation + ", "
+ searchGraph.bfsPath.get(i).yLocation, 0, 1);
LCD.drawString("Press ENTER key", 0, 2);
// block the thread until a button is pressed
buttons.waitForAnyPress();
if (i == searchGraph.bfsTraverse.size() - 1) {
navbot.goTo(searchGraph.bfsTraverse.get(i).to.yLocation,
searchGraph.bfsTraverse.get(i).to.xLocation);
LCD.drawString("At location "
+ searchGraph.bfsTraverse.get(i).to.cityName, 0, 0);
// block the thread until a button is
// pressed
buttons.waitForAnyPress();
}
}
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch6p2_Link.java
//Represents a link between two GraphNodes
//******************************************************************
public class ch6p2_Link {
ch6p2_GraphNode from;
ch6p2_GraphNode to;
// boolean skip is used for traversal to determine if the path // has already
// been visited or not
boolean skip;
public ch6p2_Link(ch6p2_GraphNode from, ch6p2_GraphNode to) {
this.from = from;
this.to = to;
skip = false;
}
}
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 //ch6p2_GraphNode.java
//Represents name and coordinates of a node on a graph
//******************************************************************************************
public class ch6p2_GraphNode {
String cityName;
int xLocation, yLocation;
public ch6p2_GraphNode(String cityName, int xLocation, int yLocation) {
this.cityName = cityName;
this.xLocation = xLocation;
this.yLocation = yLocation;
}
public String toString() {
return cityName + " ("+xLocation +"," + yLocation + ")";
}
}
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch6p2_Graph.java
//Represents GraphNodes connected through Links, including method //for doing a breadth-first
//search traversal of the graph.
//the method bfsTraverse creates a bfsPath list which the robot will //follow to demonstrate
//how the breadth-first search works.
//******************************************************************************************
import java.util.ArrayList;
public class ch6p2_Graph {
// nodes and links define the physical creation of your Graph
ArrayList<ch6p2_GraphNode> nodes;
ArrayList<ch6p2_Link> links;
// a list used for traversing
ArrayList<ch6p2_Link> bfsTraverse;
// List used to define where you would like to move
ArrayList<ch6p2_GraphNode> bfsPath = new ArrayList<ch6p2_GraphNode>();
// Constructor of ch6p2_Graph class
public ch6p2_Graph() {
nodes = new ArrayList<ch6p2_GraphNode>();
links = new ArrayList<ch6p2_Link>();
bfsTraverse = new ArrayList<ch6p2_Link>();
}// end constructor
// addNode()
// Add a city node to the graph
public void addNode(ch6p2_GraphNode node) {
nodes.add(node);
}// end addNode
// addLink
// Add link to the graph
public void addLink(ch6p2_Link link) {
links.add(link);
}// end addLink
// bfsTraverse()
// perform breadth-first search on the graph
public void bfsTraverse(ch6p2_GraphNode from, ch6p2_GraphNode to) {
ArrayList<ch6p2_Link> resetList = new ArrayList<ch6p2_Link>();
boolean matched = false;
ch6p2_Link l;
// determine if there is a link between from and to
// if there is a match then add the link to the
// travelStack and
// add the nodes to bfsPath
// This will ultimately repeated by the end of the search
matched = match(from, to);
// add to bfsTraverse if match is found
if (matched) {
bfsTraverse.add(new ch6p2_Link(from, to));
return;
}
// continue while links exist
while ((l = find(from)) != null) {
resetList.add(l);
if ((matched = match(l.to, to))) {
resetList.add(new ch6p2_Link(l.from, to));
bfsTraverse.add(new ch6p2_Link(from, l.to));
bfsTraverse.add(new ch6p2_Link(l.to, to));
return;
}
}
for (int i = resetList.size(); i != 0; i--) {
resetSkip((ch6p2_Link) resetList.remove(i - 1));
}
// if you find a new connection then you could add it // to the travelStack
// and
// and the start node to bfsPath
// recursively call bfsTraverse with the link's to as // start and our
// destination as the end
l = find(from);
if (l != null) {
bfsTraverse.add(new ch6p2_Link(from, to));
bfsPath.add(new ch6p2_GraphNode(from.cityName, from.xLocation,
from.yLocation));
bfsTraverse(l.to, to);
}
// backtrack if you cannot find a new connection
else if (bfsTraverse.size() > 0) {
l = (ch6p2_Link) bfsTraverse.remove(bfsTraverse.size() - 1);
bfsPath.remove(bfsPath.size() - 1);
bfsTraverse(l.from, l.to);
}
}// end bfsTraverse
// resetSkip
// when backtracking reset skip flag so we can visit nodes
// again
public void resetSkip(ch6p2_Link l) {
for (int i = 0; i < links.size(); i++) {
if (links.get(i).from.equals(l.from)
&& links.get(i).to.equals(l.to)) {
links.get(i).skip = false;
}
}
}
// match() method is used to determine if there is a link
// between a starting
// node and an ending node
public boolean match(ch6p2_GraphNode from, ch6p2_GraphNode to) {
// iterate through list of links
for (int x = links.size() - 1; x >= 0; x--) {
if (links.get(x).from.equals(from) && links.get(x).to.equals(to)
&& !links.get(x).skip) {
links.get(x).skip = true;
return true;
}
}
return false;
}// end match
// find() method is used to
// find the next link to try exploring
public ch6p2_Link find(ch6p2_GraphNode from) {
// iterate through the list of links
for (int x = 0; x < links.size(); x++) {
// link found
if (links.get(x).from.equals(from) && !links.get(x).skip) {
ch6p2_Link l = new ch6p2_Link(links.get(x).from,
links.get(x).to);
links.get(x).skip = true;
// mark this link as used so we don't
// match it again
return l;
}
}
return null; // not found
}// end find()
}// end Graph.java
摘要
在本章中，你学习了广度优先搜索算法的基本原理，以及如何在实践中应用它来解决搜索程序。您还学习了如何基于广度优先搜索算法和您在前面章节中学习的导航类来构建问题解决代理，在导航类中，问题解决代理智能地找到从起点到任何目的地的路线路径。
在下一章中，您将了解启发式搜索策略背后的基本思想，学习如何在 leJOS EV3 上实现爬山算法，然后将实现的爬山算法集成到基于 leJOS 的机器人系统中进行定位和路径规划。
七、爬山搜索及其在 Lego Mindstorms 中的实现
正如在第五章中向您介绍深度优先搜索(DFS)算法和在第六章中向您介绍广度优先搜索算法一样，在本章中，您将学习启发式搜索策略背后的基本思想以及如何实现爬山算法，这是 leJOS EV3 中最典型的启发式方法之一。具体而言，本章将涵盖以下主题:
- 一种新的爬山搜索算法，可用于构建任意树结构。
- 在基于 leJOS 的机器人系统中应用和集成所提出的爬山算法用于定位和路径规划，这增强了 leJOS 系统中现有的寻路方法。
启发式搜索简介
问题求解是许多人工智能相关应用中的基本问题之一。通常，有两类问题。第一个问题可以通过使用某种确定性过程来解决，例如绝对值的计算，例如根据坐标计算两个节点之间的距离。对于计算机来说，解决这类问题非常容易，因为它们只需将问题转化为一个方程，然后就可以执行了。然而，第二类问题并不总是导致直接确定的解决方案。这种情况在现实世界中经常发生，很多问题只有通过使用搜索策略来寻找解决方案才能得到解决，比如广度优先搜索或深度优先搜索，你已经在第 5 和 6 章看到了。
随着本章的深入，您将了解启发式搜索方法。通过下面的例子，你将了解到人工智能搜索策略中使用的一些表示和术语:
假设你在学校大楼的某个地方丢了一本书。该建筑如图 7-1 所示。
图 7-1。
Building topology
你正站在“你”街区的走廊前面。当你开始搜索时，你检查 R8 房间。没有找到你的书，你回到走廊，然后检查房间 R7。没有找到你的书，你又回到走廊，等等。通过检查每一个房间，并来回回到走廊，你终于在 R2 房间找到了你的书。这样的场景可以很容易地转换成如下图 7-2 所示的图形:
图 7-2。
Graphic representation of your search strategy
正如你在第 5 和 6 章中看到的，用图形格式表示搜索问题是有益的，因为它提供了一种方便的方式来描述如何找到解决方案。纵观本书，在介绍 AI 搜索算法时，你会发现使用了以下术语:
- 节点 A 离散点:例如图 7-2 中的走廊。
- 终端节点:路径结束的节点:例如图 7-2 中的房间 R8。
- 图中所有节点的集合。
- 搜索要到达的节点。
- 启发式实用函数，用于确定任何特定节点是否是比另一个更好的下一个选择。
- 解决方案:一组连接并指向目的地的节点。
在上面的丢书场景中，大楼中的每个教室都被称为一个节点。整个建筑，包括八个教室，都是搜索空间。结果，目标是 R2 房间。最后，解决方案路径如图 7-2 虚线箭头所示。教室 R1、R2、R3、R4、R5、R6、R7 和 R8 是终端节点，因为它们不通向任何地方。试探法在图中没有表示。相反，它们是你可以用来帮助你更好地找到一条道路的技术。
鉴于上面的例子，你可能认为寻找解决方案一点也不难；也就是说，您只需从起点开始，然后逐个搜索节点，以达到您的目标。只有在上面丢书的例子这种极其简单的情况下才成立，因为搜索空间太小了(也就是只有八个节点，你最多要尝试八次才能找到解)。然而，在现实世界中，搜索空间通常非常大。例如，想想新罕布什尔州有多少个城市，而整个美国有多少个城市？随着搜索空间的增长，到达目标的可能路径的数量也在增长。此外，当向搜索空间添加另一个节点时，一个重要的问题是，它通常会添加多条路径，因此不是线性的。换句话说，当搜索空间变大时，达到目标的潜在途径的数量会以非线性方式增加。
在非线性情况下，可能路径的数量会很快变得非常大。例如，考虑您可以用来排列三个对象 A、B 和 c 的方法的数量。总共六种可能的排列如下:
| A | B | C | | A | C | B | | B | C | A | | B | A | C | | C | B | A | | C | A | B |当你再增加一个对象时，你会看到可能的排列总数是 7，因为 N 个对象可以排列的方法数等于 N！(N 阶乘)。因此，如果你有四个对象要排列，那么就会有四个！排列(即 24 种排列)。对于五个对象，该数目变成 120 个排列，而对于六个对象，该数目增加到 720 个排列。对于 100 个对象，可能的排列数量是巨大的:93326215443944152681699238856266700490715968264381621468592963895217599993229915608941463976156518286253697920827223758251185210916864000000000000000000000000。在这种情况下，检查所有节点的穷举搜索将不起作用，因为它消耗太多时间和太多计算资源。因此，基于人工智能的搜索技术对于搜索解决方案至关重要。三种最基本的技术是深度优先搜索、宽度优先搜索和爬山搜索。
比较和评估各种基于人工智能的搜索技术的性能可能非常复杂。在本书中，我们主要关注两个衡量标准:(1)找到解决方案有多快，以及(2)解决方案有多好。当您试图以最小的努力找到一个解决方案时，第一个衡量标准——即找到一个解决方案的速度——尤其重要，它通常受到搜索空间大小和在找到解决方案的过程中实际遍历的节点数量的影响。这是因为从死胡同回溯是耗时的，并且你想要一个很少回溯的搜索。在其他情况下，解决方案的质量更重要。在基于人工智能的搜索中，找到最佳解决方案和找到好的解决方案是有区别的。寻找最佳解决方案需要彻底的搜索，因为这是知道已经找到最佳解决方案或者已经实现全局优化的唯一方式。另一方面，找到一个好的解决方案意味着找到一个在定义的约束集内的解决方案，但是如果存在一个更好的解决方案，并且您获得的可能是一个局部最优的解决方案，这并不重要。
正如你在第五章中看到的，当第一次尝试排除回溯时，深度优先方法可以找到一个好的解决方案。然而，当我们按照第五章的图 5-2 所示重新组织图形时，寻找解决方案可能会涉及大量的回溯。因此，在第五章中介绍的例子的结果不能推广。此外，当探索一个特别长的末端无解的分支时，深度优先搜索的性能会相当差。在这种情况下，深度优先搜索会浪费探索这个分支和回溯到目标的时间。
在第六章举例说明的例子中，广度优先搜索表现非常好，找到了一个合理的解决方案。类似于深度优先搜索，这种解决方案不能通用化，因为要找到的第一条路径取决于节点信息的实际物理组织。比较深度优先搜索和广度优先搜索，您可以在相同的搜索空间中找到不同的路径。当目标不太深入搜索空间时，广度优先搜索效果很好。然而，当目标位于几层深处时，它的效果很差。在这种情况下，广度优先搜索会在回溯过程中耗费大量精力。
基于以上所述，断言一种搜索方法总是优于另一种方法是不准确的。在许多情况下，定义问题的方式可以帮助您选择合适的搜索方法。正如您在第 5 和 6 章中所了解到的，深度优先搜索方法和广度优先搜索方法都不试图对搜索空间中的一个节点是否比另一个节点更接近目标做出任何合理的猜测。相反，这两种搜索策略只是使用指定的模式从一个节点移动到下一个节点，直到最终达到目标。为了提高搜索更快达到目标的概率，您需要在搜索算法中添加启发式功能。
试探法只是增加搜索向正确方向前进的可能性的规则。例如，假设你在新罕布什尔州的白山迷路了。你仍然想爬上山顶，而且你知道在山顶上非常寒冷和多风。白山上的树林非常茂密，以至于你看不到远处。此外，树太大了，不能爬上去四处看看。然而，你知道峰顶的温度会比你现在的位置更冷，所以当你接近峰顶时，你会开始“感觉”到它，因为它变得越来越冷。
你从上山开始。你知道你正朝着正确的方向前进，因为你正沿着一条每走一步都感觉更冷的道路前进。随着你的移动，你可以感觉到温度在下降。最后，你到达了山顶。在这种情况下，用于寻找顶点的启发式信息并不能保证成功。然而，它增加了早期成功的可能性，引导你快速达到目标。
启发式搜索的基本思想是，你试着把注意力集中在那些似乎能让你更接近目标的路径上，而不是尝试所有可能的搜索路径。即使你不能确定你真的接近了你的目标，你也可以做出一个很好的猜测，启发式方法可以帮助你做出这个猜测。
在进行启发式搜索时，您需要一个效用函数，它根据您与目标状态的接近程度对搜索树中的一个节点进行评分。虽然这只是一个猜测，但它仍然有助于最大化或最小化某些约束。例如，当您应用 GPS 来设置从起点到终点的路线时，您可能希望最小化两个可能的约束。第一个是时间更快，第二个是距离最短。最短的路线不一定意味着最快的时间，反之亦然。在本章的例子中，我们将重点放在基于深度优先搜索框架的爬山搜索上，其目的是最小化称为连接数的约束。
爬山搜索概述
在爬山中，最基本的想法是朝着一个比现在更好的状态前进。因此，如果你在 A 镇，你需要到达 B 镇和 C 镇(你的最终目标是 D 镇)，那么如果 B 镇或 C 镇看起来比 A 镇更靠近 D 镇，你应该采取行动。在最陡的爬坡中，爬山搜索总是会使你的下一个州成为你当前州的最佳继任者，只有当继任者比你当前州更好时，它才会采取行动。这可以描述如下:
- 从当前状态(初始状态)开始。
- 直到当前状态=目标状态，或者当前状态没有变化，执行以下操作:
- 获取当前状态的后继者，并使用效用函数给每个后继者分配一个分数。
- 如果其中一个后继者具有比当前状态更好的分数，则将新的当前状态设置为具有最佳分数的后继者。
- 由于该算法不试图彻底测试每个节点和路径，所以除了当前状态之外，您不必维护节点。
- 当没有比当前状态本身更好的当前状态的后继者时，爬山终止。
接下来，让我们看看如何使用爬山搜索方法来解决一个寻路问题，该问题可以应用于从起始城市导航到目的城市时的 GPS 系统。假设您想使用爬山搜索算法以最少的节点数从城市 A(例如，纽约州纽约市)开车到 E(例如，马萨诸塞州波士顿)，如图 7-3 所示。假设我们使用笛卡尔坐标系如下:
- A 处的坐标是(0，0)。
- B 点的坐标是(-10，20)。
- C 点的坐标是(-15，20)。
- D 点的坐标是(0，20)。
- E 点的坐标是(0，30)。
- F 点的坐标是(-15，30)。
- 在 G 点的坐标是(10，20)。
- H 点的坐标是(10，10)。
图 7-3。
Routes in between two cities
根据下面的路线，为你制定一个计划，从 A 点出发，使用爬山搜索策略到达 E 点。
以下程序可用于通过使用爬山搜索算法为您自动安排旅行计划找到一条路径:
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch7p1_main.java
//Driver class to set up map using ch7p1_GraphNode, ch7p1_Link, and //ch7p1_Graph classes.
//Calls a hill-climbing search in ch7p1_Graph class to create //navigation path from a start
//and end node
//******************************************************************************************
public class ch7p1_main {
public static void main(String[] args) {
// These objects used to define what your graph looks // like
ch7p1_Graph searchGraph = new ch7p1_Graph();
ch7p1_GraphNode A, B, C, D, E, F, G, H;
ch7p1_Link AB, AC, AD, BD, CB, CE, CF, DE, DH, FE, GE, HG;
// define each node
A = new ch7p1_GraphNode("A", 0, 0);
B = new ch7p1_GraphNode("B", -10, 20);
C = new ch7p1_GraphNode("C", -15, 20);
D = new ch7p1_GraphNode("D", 0, 20);
E = new ch7p1_GraphNode("E", 0, 30);
F = new ch7p1_GraphNode("F", -15, 30);
G = new ch7p1_GraphNode("G", 10, 20);
H = new ch7p1_GraphNode("H", 10, 10);
// define which GraphNodes are connected
AB = new ch7p1_Link(A, B, dist(A.xLocation, A.yLocation, B.xLocation,
B.yLocation));
AC = new ch7p1_Link(A, C, dist(A.xLocation, A.yLocation, C.xLocation,
C.yLocation));
AD = new ch7p1_Link(A, D, dist(A.xLocation, A.yLocation, D.xLocation,
D.yLocation));
BD = new ch7p1_Link(B, D, dist(B.xLocation, B.yLocation, D.xLocation,
D.yLocation));
CB = new ch7p1_Link(C, B, dist(C.xLocation, C.yLocation, B.xLocation,
B.yLocation));
CE = new ch7p1_Link(C, E, dist(C.xLocation, C.yLocation, E.xLocation,
E.yLocation));
CF = new ch7p1_Link(C, F, dist(C.xLocation, C.yLocation, F.xLocation,
F.yLocation));
DE = new ch7p1_Link(D, E, dist(D.xLocation, D.yLocation, E.xLocation,
E.yLocation));
DH = new ch7p1_Link(D, H, dist(D.xLocation, D.yLocation, H.xLocation,
H.yLocation));
FE = new ch7p1_Link(F, E, dist(F.xLocation, F.yLocation, E.xLocation,
E.yLocation));
GE = new ch7p1_Link(G, E, dist(G.xLocation, G.yLocation, E.xLocation,
E.yLocation));
HG = new ch7p1_Link(H, G, dist(H.xLocation, H.yLocation, G.xLocation,
G.yLocation));
// add all nodes and links to your graph object
searchGraph.addNode(A);
searchGraph.addNode(B);
searchGraph.addNode(C);
searchGraph.addNode(D);
searchGraph.addNode(E);
searchGraph.addNode(F);
searchGraph.addNode(G);
searchGraph.addNode(H);
searchGraph.addLink(AB);
searchGraph.addLink(AC);
searchGraph.addLink(AD);
searchGraph.addLink(BD);
searchGraph.addLink(CB);
searchGraph.addLink(CE);
searchGraph.addLink(CF);
searchGraph.addLink(DE);
searchGraph.addLink(DH);
searchGraph.addLink(FE);
searchGraph.addLink(GE);
searchGraph.addLink(HG);
// run hill-climbing search to get from start to
// destination
searchGraph.hillTraverse(
searchGraph.nodes.get(searchGraph.nodes.indexOf(A)),
searchGraph.nodes.get(searchGraph.nodes.indexOf(E)));
// display path created using hillTraverse
// This will be display the path from start to
// destination
System.
out
.println("the path from your current city to the destination city is: ");
for (int i = searchGraph.hillPath.size() - 1; i >= 0; i--) {
int count = 0;
if (i != 0) {
for (int j = searchGraph.hillPath.size() - 1; j >= 0; j--) {
if (searchGraph.hillPath.get(i).cityName == searchGraph.hillPath
.get(j).cityName)
count++;
}
if (count == 1)
System.out.print(searchGraph.hillPath.get(i).cityName
+ "->");
} else
System.out.print(searchGraph.hillPath.get(i).cityName);
}
}
static int dist(int x1, int y1, int x2, int y2) {
int distance = 0;
distance = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
return distance;
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch7p1_Link.java
//Represents a link between two GraphNodes
//******************************************************************
public class ch7p1_Link {
ch7p1_GraphNode from;
ch7p1_GraphNode to;
int distance;
// boolean skip is used for traversal to determine if the path // has already
// been visited or not
boolean skip;
public ch7p1_Link(ch7p1_GraphNode from, ch7p1_GraphNode to, int d) {
this.from = from;
this.to = to;
distance = d;
skip = false;
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3
// ch7p1_GraphNode.java
//Represents name and coordinates of a node on a graph
//******************************************************************
public class ch7p1_GraphNode {
String cityName;
int xLocation, yLocation;
public ch7p1_GraphNode(String cityName, int xLocation, int yLocation) {
this.cityName = cityName;
this.xLocation = xLocation;
this.yLocation = yLocation;
}
public String toString() {
return cityName + " ("+xLocation +"," + yLocation + ")";
}
}
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch7p1_Graph.java
//Represents GraphNodes connected through Links, including method //for doing a hill climbing
//search traversal of the graph.
//the method hillTraverse creates a hillPath list which the robot //will follow to demonstrate
//how the hill-climbing search works.
//******************************************************************************************
import java.util.ArrayList;
import java.util.Stack;
public class ch7p1_Graph {
// nodes and links define the physical creation of your Graph
ArrayList<ch7p1_GraphNode> nodes;
ArrayList<ch7p1_Link> links;
// Two lists used for traversing
ArrayList<ch7p1_GraphNode> hillTraverse;
Stack<ch7p1_Link> travelStack = new Stack<ch7p1_Link>();
// List used to define where you would like to move
ArrayList<ch7p1_GraphNode> hillPath = new ArrayList<ch7p1_GraphNode>();
// Constructor of ch7p1_Graph class
public ch7p1_Graph() {
nodes = new ArrayList<ch7p1_GraphNode>();
links = new ArrayList<ch7p1_Link>();
hillTraverse = new ArrayList<ch7p1_GraphNode>();
}// end constructor
// addNode()
// Add a city node to the graph
public void addNode(ch7p1_GraphNode node) {
nodes.add(node);
}// end addNode
// addLink
// Add link to the graph
public void addLink(ch7p1_Link link) {
links.add(link);
}// end addLink
// hillTraverse()
// perform hill-climbing search on the graph
public void hillTraverse(ch7p1_GraphNode from, ch7p1_GraphNode to) {
// boolean matched;
int distance;
ch7p1_Link found;
// determine if there is a link between from and to
// if there is a match then add the link to the
// travelStack and
// add the nodes to hillPath
// This will ultimately repeated by the end of the
// search
distance = match(from, to);
if (distance != 0) {
travelStack.push(new ch7p1_Link(from, to, distance));
hillPath.add(new ch7p1_GraphNode(to.cityName, to.xLocation,
to.yLocation));
hillPath.add(new ch7p1_GraphNode(from.cityName, from.xLocation,
from.yLocation));
return;
}
// if there is no match found you could another path
// findings
found = find(from);
// if you find a new connection then you could add it // to the travelStack
// and
// and the start node to hillPath
// recursively call hillTraverse with the link's to as // start and our
// destination as the end
if (found != null) {
travelStack.push(new ch7p1_Link(from, to, found.distance));
hillTraverse(found.to, to);
hillPath.add(new ch7p1_GraphNode(from.cityName, from.xLocation,
from.yLocation));
}
// backtrack if you cannot find a new connection
else if (travelStack.size() > 0) {
found = travelStack.pop();
hillTraverse(found.from, found.to);
hillPath.add(new ch7p1_GraphNode(from.cityName, from.xLocation,
from.yLocation));
}
}// end hillTraverse()
// find() method is used to
// find the next link to try exploring
public ch7p1_Link find(ch7p1_GraphNode from) {
int pos = -1;
int dist = 0;
// iterate through the list of links
for (int a = 0; a < links.size(); a++) {
if (links.get(a).from.equals(from) && !links.get(a).skip) {
// Use the longest flight.
if (links.get(a).distance > dist) {
pos = a;
dist = links.get(a).distance;
}
}
}
// link found
if (pos != -1) {
// mark this link as used so we don't match it
// again
links.get(pos).skip = true;
ch7p1_Link foundList = new ch7p1_Link(links.get(pos).from,
links.get(pos).to, links.get(pos).distance);
return foundList;
}
return null; // not found
}// end find()
// match() method is used to determine if there is a link
// between a starting
// node and an ending node
public int match(ch7p1_GraphNode from, ch7p1_GraphNode to) {
// iterate through list of links
for (int a = links.size() - 1; a >= 0; a--) {
if (links.get(a).from.equals(from) && links.get(a).to.equals(to)
&& !links.get(a).skip) {
links.get(a).skip = true;
return links.get(a).distance;
}
}
return 0;
}// end match()
}// end Graph.java
给定startCity A 和endCity E，运行程序的结果如下:
the path from your current city to the destination city is:
A->C->E
此结果显示了使用爬山搜索策略遍历图形的准确路径，不包括中间回溯节点。
基于莱霍斯·EV3 的爬山算法
在这一节中，你需要为你的机器人开发一个程序，使它能够在起始节点 A 和目的节点 E 之间的路径上行进，如图 7-3 所示，使用最少的节点数，并使用爬山搜索算法。
您的程序应至少在 LCD 上显示目的地的坐标，然后显示一条消息“Press ENTER key to continue”当回车键被按下时，你的机器人应该移动到下一个节点。例如，假设你的机器人从A (0,0)出发，想要探索一条通往E (0,30)的道路。假设你的机器人使用爬山搜索通过最少节点的路径是A -> C -> E。在起点 A，您的程序应该执行以下操作:
- 在 LCD 上显示目的地的坐标
C(-15, 20)。 - 显示信息“
Press ENTER key to continue”。 - 去坐标为-15，20 的地方。
- 在 LCD 上显示目的地的坐标
E(0, 30)。 - 显示信息“
Press ENTER key to continue.” - 转到坐标为 0，30 的位置。
而且，你的问题应该有一个名为 destination 的字符串，这样它就足够智能，当改变 destination 的值时，你的机器人可以探索一条从起始节点 A 到新的目的节点的新路径，机器人将通过最少数量的节点到达目的节点。(我们假设起始节点总是 A，所以 from 字符串可以硬编码。)
以下程序表示基于 leJOS 的爬山算法的实现，该算法被设计成探索从节点 A 到目的节点 E 的路径:
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch7p2_main.java
//Driver class to set up map using ch7p2_GraphNode, ch7p2_Link, and //ch7p2_Graph classes.
//Calls a hill-climbing search in ch7p2_Graph class to create //navigation path from a start
//and end node and then robots will follow the path to move from //start node
//to destination node
//******************************************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.MovePilot;
import lejos.robotics.navigation.Navigator;
public class ch7p2_main {
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(
MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(
MotorPort.C);
public static void main(String[] args) {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
// setup the wheel diameter of left (and right) motor // in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center // of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot ev3robot = new MovePilot(chassis);
Navigator navbot = new Navigator(ev3robot);
// These objects used to define what your graph looks // like
ch7p2_Graph searchGraph = new ch7p2_Graph();
ch7p2_GraphNode A, B, C, D, E, F, G, H;
ch7p2_Link AB, AC, AD, BD, CB, CE, CF, DE, DH, FE, GE, HG;
// define each node
A = new ch7p2_GraphNode("A", 0, 0);
B = new ch7p2_GraphNode("B", -10, 20);
C = new ch7p2_GraphNode("C", -15, 20);
D = new ch7p2_GraphNode("D", 0, 20);
E = new ch7p2_GraphNode("E", 0, 30);
F = new ch7p2_GraphNode("F", -15, 30);
G = new ch7p2_GraphNode("G", 10, 20);
H = new ch7p2_GraphNode("H", 10, 10);
// define which GraphNodes are connected
AB = new ch7p2_Link(A, B, dist(A.xLocation, A.yLocation, B.xLocation,
B.yLocation));
AC = new ch7p2_Link(A, C, dist(A.xLocation, A.yLocation, C.xLocation,
C.yLocation));
AD = new ch7p2_Link(A, D, dist(A.xLocation, A.yLocation, D.xLocation,
D.yLocation));
BD = new ch7p2_Link(B, D, dist(B.xLocation, B.yLocation, D.xLocation,
D.yLocation));
CB = new ch7p2_Link(C, B, dist(C.xLocation, C.yLocation, B.xLocation,
B.yLocation));
CE = new ch7p2_Link(C, E, dist(C.xLocation, C.yLocation, E.xLocation,
E.yLocation));
CF = new ch7p2_Link(C, F, dist(C.xLocation, C.yLocation, F.xLocation,
F.yLocation));
DE = new ch7p2_Link(D, E, dist(D.xLocation, D.yLocation, E.xLocation,
E.yLocation));
DH = new ch7p2_Link(D, H, dist(D.xLocation, D.yLocation, H.xLocation,
H.yLocation));
FE = new ch7p2_Link(F, E, dist(F.xLocation, F.yLocation, E.xLocation,
E.yLocation));
GE = new ch7p2_Link(G, E, dist(G.xLocation, G.yLocation, E.xLocation,
E.yLocation));
HG = new ch7p2_Link(H, G, dist(H.xLocation, H.yLocation, G.xLocation,
G.yLocation));
// add all nodes and links to your graph object
searchGraph.addNode(A);
searchGraph.addNode(B);
searchGraph.addNode(C);
searchGraph.addNode(D);
searchGraph.addNode(E);
searchGraph.addNode(F);
searchGraph.addNode(G);
searchGraph.addNode(H);
searchGraph.addLink(AB);
searchGraph.addLink(AC);
searchGraph.addLink(AD);
searchGraph.addLink(BD);
searchGraph.addLink(CB);
searchGraph.addLink(CE);
searchGraph.addLink(CF);
searchGraph.addLink(DE);
searchGraph.addLink(DH);
searchGraph.addLink(FE);
searchGraph.addLink(GE);
searchGraph.addLink(HG);
// run hill-climbing search to get from start to
// destination
searchGraph.hillTraverse(
searchGraph.nodes.get(searchGraph.nodes.indexOf(A)),
searchGraph.nodes.get(searchGraph.nodes.indexOf(E)));
// block the thread until a button is pressed
buttons.waitForAnyPress();
// Robot moves through path from start to destination // by using
// hillTraverse
for (int i = searchGraph.hillPath.size() - 1; i >= 0; i--) {
int count = 0;
for (int j = searchGraph.hillPath.size() - 1; j >= 0; j--) {
if (searchGraph.hillPath.get(i).cityName == searchGraph.hillPath
.get(j).cityName)
count++;
}
if (count == 1) {
// go to node
navbot.goTo(searchGraph.hillPath.get(i).xLocation,
searchGraph.hillPath.get(i).yLocation);
LCD.clear();
// display current location
LCD.drawString("At location "
+ searchGraph.hillPath.get(i).cityName + ", ", 0, 0);
LCD.drawString(searchGraph.hillPath.get(i).xLocation + ", "
+ searchGraph.hillPath.get(i).yLocation, 0, 1);
LCD.drawString("Press ENTER key", 0, 2);
// block the thread until a button is
// pressed
buttons.waitForAnyPress();
}
}
}
static int dist(int x1, int y1, int x2, int y2) {
int distance = 0;
distance = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
return distance;
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch7p2_Link.java
//Represents a link between two GraphNodes);
//******************************************************************
public class ch7p2_Link {
ch7p2_GraphNode from;
ch7p2_GraphNode to;
int distance;
// boolean skip is used for traversal to determine if the path // has already
// been visited or not
boolean skip;
public ch7p2_Link(ch7p2_GraphNode from, ch7p2_GraphNode to, int d) {
this.from = from;
this.to = to;
distance = d;
skip = false;
}
}
//************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 //ch7p1_GraphNode.java
//Represents name and coordinates of a node on a graph
//************************************************************************
public class ch7p2_GraphNode {
String cityName;
int xLocation, yLocation;
public ch7p2_GraphNode(String cityName, int xLocation, int yLocation) {
this.cityName = cityName;
this.xLocation = xLocation;
this.yLocation = yLocation;
}
public String toString() {
return cityName + " (" + xLocation + "," + yLocation + ")";
}
}
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch7p2_Graph.java
//Represents GraphNodes connected through Links, including method //for doing a hill climbing
//search traversal of the graph.
//the method hillTraverse creates a hillPath list which the robot //will follow to demonstrate
//how the hill-climbing search works.
//******************************************************************************************
import java.util.ArrayList;);
import java.util.Stack;
public class ch7p2_Graph {
// nodes and links define the physical creation of your Graph
ArrayList<ch7p2_GraphNode> nodes;
ArrayList<ch7p2_Link> links;
// Two lists used for traversing
ArrayList<ch7p2_GraphNode> hillTraverse;
Stack<ch7p2_Link> travelStack = new Stack<ch7p2_Link>();
// List used to define where you would like to move
ArrayList<ch7p2_GraphNode> hillPath = new ArrayList<ch7p2_GraphNode>();
// Constructor of ch7p2_Graph class
public ch7p2_Graph() {
nodes = new ArrayList<ch7p2_GraphNode>();
links = new ArrayList<ch7p2_Link>();
hillTraverse = new ArrayList<ch7p2_GraphNode>();
}// end constructor
// addNode()
// Add a city node to the graph
public void addNode(ch7p2_GraphNode node) {
nodes.add(node);
}// end addNode
// addLink
// Add link to the graph
public void addLink(ch7p2_Link link) {
links.add(link);
}// end addLink
// hillTraverse()
// perform hill-climbing search on the graph
public void hillTraverse(ch7p2_GraphNode from, ch7p2_GraphNode to) {
// boolean matched;
int distance;
ch7p2_Link found;
// determine if there is a link between from and to
// if there is a match then add the link to the
// travelStack and
// add the nodes to hillPath
// This will ultimately repeated by the end of the
// search
distance = match(from, to);
if (distance != 0) {
travelStack.push(new ch7p2_Link(from, to, distance));
hillPath.add(new ch7p2_GraphNode(to.cityName, to.xLocation,
to.yLocation));
hillPath.add(new ch7p2_GraphNode(from.cityName, from.xLocation,
from.yLocation));
return;
}
// if there is no match found you could another path
// findings
found = find(from);
// if you find a new connection then you could add it // to the travelStack
// and
// and the start node to hillPath
// recursively call hillTraverse with the link's to as // start and our
// destination as the end
if (found != null) {
travelStack.push(new ch7p2_Link(from, to, found.distance));
hillTraverse(found.to, to);
hillPath.add(new ch7p2_GraphNode(from.cityName, from.xLocation,
from.yLocation));
}
// backtrack if you cannot find a new connection
else if (travelStack.size() > 0) {
found = travelStack.pop();
hillTraverse(found.from, found.to);
hillPath.add(new ch7p2_GraphNode(from.cityName, from.xLocation,
from.yLocation));
}
}// end hillTraverse()
// find() method is used to
// find the next link to try exploring);
public ch7p2_Link find(ch7p2_GraphNode from) {
int pos = -1;
int dist = 0;
// iterate through the list of links
for (int a = 0; a < links.size(); a++) {
if (links.get(a).from.equals(from) && !links.get(a).skip) {
// Use the longest flight.
if (links.get(a).distance > dist) {
pos = a;
dist = links.get(a).distance;
}
}
}
// link found
if (pos != -1) {
// mark this link as used so we don't match it
// again
links.get(pos).skip = true;
ch7p2_Link foundList = new ch7p2_Link(links.get(pos).from,
links.get(pos).to, links.get(pos).distance);
return foundList;
}
return null; // not found
}// end find()
// match() method is used to determine if there is a link
// between a starting
// node and an ending node
public int match(ch7p2_GraphNode from, ch7p2_GraphNode to) {
// iterate through list of links
for (int a = links.size() - 1; a >= 0; a--) {
if (links.get(a).from.equals(from) && links.get(a).to.equals(to)
&& !links.get(a).skip) {
links.get(a).skip = true;
return links.get(a).distance;
}
}
return 0;
}// end match()
}// end Graph.java);
摘要
在这一章中，你学习了启发式搜索和爬山算法的基础。你现在知道如何在实践中应用爬山搜索算法来解决一个搜索程序。您还学习了如何基于爬山搜索算法和导航类来构建解决问题的代理，在前面的章节中，这些代理使用最短的连接数智能地找到从起点到任何目的地的路线。
在下一章中，您将学习 Dijkstra 算法，该算法可用于寻找最优解，而不是爬山中的启发式函数所定义的(或“任何”)解。此外，您还将学习如何将 Dijkstra 的算法集成到基于 leJOS 的机器人系统中，以进行定位和路径规划。
八、Dijkstra 算法及其在 Lego Mindstorms 上的实现
继第七章对启发式搜索算法的讨论之后，本章将讨论 Dijkstra 算法。该算法可用于寻找最优解，而不是由爬山算法中的启发式函数定义的“或任何”解。特别是，本章将涵盖以下主题:
- 一个新的 Dijkstra 的搜索算法，可以应用于建立任意树结构一般。
- 在基于 leJOS 的机器人系统中应用和集成所提出的 Dijkstra 算法用于定位和路径规划，这增强了 leJOS 系统中现有的寻路方法。
Dijkstra 算法简介
在第七章中讨论的爬山算法试图最小化连接的数量。如你所知，这样的爬山搜索找到了一条好的路线——不是最好的，但是可以接受的。此外，迄今为止介绍的所有先前的搜索算法(包括 BSF 和 DFS)都能够确定一个解决方案:即，任何解决方案，不一定是最佳解决方案或者甚至是“好的”解决方案。定义试探法有助于提高找到好的解决方案的可能性；然而，没有作出任何努力来确保找到最佳解决方案。为了获得最优解，即最佳解，需要应用众所周知的 Dijkstra 算法:
在任何给定的图中和在任何起始节点，Dijkstra 的算法发现从起始节点到所有其他节点的最短路径。
图 8-1 显示了表示连接节点的图形:节点是标有 A-J 的蓝色圆圈。路径是连接两个节点的蓝色线，每条路径旁边都有一个关联的距离。注意:长度不是按比例的。
图 8-1。
Graphic representation of connected nodes
节点 A 是您的起始节点，您希望找到到达图中所有其他节点的最短路径。为此，您需要生成一个表。此表包含从起始节点 a 的角度到图形中所有节点的距离。
如表 8-1 所示，距离的初始条目都被设置为无穷大(或某个名义上的最大值)。这确保了找到的任何路径都比存储在表中的初始值短。
表 8-1。
Initial entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | | --- | --- | | B | 无限的 | | C | 无限的 | | D | 无限的 | | E | 无限的 | | F | 无限的 | | G | 无限的 | | H | 无限的 | | 我 | 无限的 | | J | 无限的 |节点 A 是起始节点，因此您将首先检查离开该节点的所有可能路径。选项如表 8-2 所示。
表 8-2。
Distance to nodes (from Node A) accessible from Node A
| 结节 | 从节点 A 到节点的距离 | | --- | --- | | B | seven | | C | Two | | D | three | | E | Seventeen |这些值用于更新表 8-1 ，因此您现在有了表 8-3 。
表 8-3。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | | --- | --- | | B | seven | | C | Two | | D | three | | E | Seventeen | | F | 无限的 | | G | 无限的 | | H | 无限的 | | 我 | 无限的 | | J | 无限的 |图 8-2 显示了红色标记的路线。从节点 a 有四条路径。但是，这些路径还不能保证是最短路径。为了确定你已经确定了最短的路径，你必须继续前进。
图 8-2。
Graphic representation in which starting node A has been visited
算法中的下一步是前往距离节点 a 最近的节点。在这种情况下，即节点 C。在节点 C，您有到节点 B、G 和 j 的可用路径。计算距离时，您必须确定它们与节点 a 的距离。新距离如表 8-4 所示。
表 8-4。
Distance to nodes accessible from Node A
| 结节 | 从节点 A 到节点的距离 | | --- | --- | | B | five | | G | Thirty-one | | J | nine |然后将这些值与表 8-3 中的值进行比较。您会发现这两个值都小于表中存储的当前值。这样，工作台 8-3 转换为工作台 8-5 如下。
表 8-5。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | | --- | --- | | B | five | | C | Two | | D | three | | E | Seventeen | | F | 无限的 | | G | Thirty-one | | H | 无限的 | | 我 | 无限的 | | J | nine |这一步说明了 Dijkstra 算法的一个优点:到 Node B 的路由不是最直接的路由，但却是最短的。Dijkstra 的算法可以找到最短的路线，即使这条路线不是最直接的。
同样，已经检查了从节点 C 可访问的所有路径，并且已经更新了路径表。节点 C 被标记为已被访问，如图 8-3 所示。
图 8-3。
Graphic representation in which Node C has been visited
在 Dijkstra 的算法中，被访问的节点永远不会被重新访问。此外，一旦节点被标记为已访问，到该节点的路径就是从初始节点开始的最短路径。
因此，您应该向您的表中添加另一列，如表 8-6 所示。
表 8-6。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | 访问 | | --- | --- | --- | | B | five | 不 | | C | Two | 是 | | D | three | 不 | | E | Seventeen | 不 | | F | 无限的 | 不 | | G | Thirty-one | 不 | | H | 无限的 | 不 | | 我 | 无限的 | 不 | | J | nine | 不 |当这些值被更新时，伴随这些距离的路线也需要被存储。再次检查路径表，并找到到未被访问的节点的最短路径。该节点成为下一个当前节点。在这种情况下，它是节点 D。从节点 D 可以使用以下路径:
表 8-7。
Distance to nodes accessible from Node A
| 结节 | 从节点 A 到节点的距离 | | --- | --- | | J | Eleven | | 我 | Sixteen | | E | eight |所有路径的表被更新以反映这一点，并且节点 D 被标记为已访问。这也锁定了到节点 D 的最短路径。
从表 8-8 中可以看出，距离节点 A 最近的节点是节点 B。接下来检查来自节点 B 的所有路径。在本例中，您有一条到标记为已访问的节点(节点 C)的路径。我们已经知道，到节点 C 的路径是最短的。
表 8-8。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | 访问 | | --- | --- | --- | | B | five | 不 | | C | Two | 是 | | D | three | 是 | | E | eight | 不 | | F | 无限的 | 不 | | G | Thirty-one | 不 | | H | 无限的 | 不 | | 我 | Sixteen | 不 | | J | nine | 不 |如图 8-4 所示，当您检查路径时，从节点 B 可访问的唯一其他节点是节点 f。这将更新我们的路径，如表 8-9 所示:
表 8-9。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | 访问 | | --- | --- | --- | | B | five | 是 | | C | Two | 是 | | D | three | 是 | | E | eight | 不 | | F | nine | 不 | | G | Thirty-one | 不 | | H | 无限的 | 不 | | 我 | Sixteen | 不 | | J | nine | 不 |
图 8-4。
Graphic representation with Node C and Node D marked visited
表 8-9 再次告诉我们，您要访问的下一个节点是节点 e。然后，您将路径相加，并将这些节点标记为已访问。接下来检查路径，从节点 E 可访问的唯一其他节点是节点 I。从节点 A 到节点 I 的距离是 27，大于表中所示的距离。因此，在这一轮中没有对表的更新，节点 E 被标记为已访问。继续看表 8-9 ，你知道下一个要访问的节点是节点 F。如图 8-4 所示，当你检查路径时，从节点 F 唯一可访问的另一个节点是节点 g。这更新了如表 8-10 所示的路径。
表 8-10。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | 访问 | | --- | --- | --- | | B | five | 是 | | C | Two | 是 | | D | three | 是 | | E | eight | 是 | | F | nine | 是 | | G | Nineteen | 不 | | H | 无限的 | 不 | | 我 | Sixteen | 不 | | J | nine | 不 |表 8-10 告诉我们你要访问的下一个节点是节点 j。然后你把路径加起来，把节点标记为已访问。接下来检查路径，从节点 J 可访问的唯一其他节点是节点 h。这将更新路径，如表 8-11 所示。
表 8-11。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | 访问 | | --- | --- | --- | | B | five | 是 | | C | Two | 是 | | D | three | 是 | | E | eight | 是 | | F | nine | 是 | | G | Nineteen | 不 | | H | Twenty-seven | 不 | | 我 | Sixteen | 不 | | J | nine | 是 |表 8-11 告诉我们您要访问的下一个节点是节点 I。接下来，您将路径相加，并将节点标记为已访问。当您检查路径时，从节点 I 可访问的唯一其他节点是节点 h。这将更新路径，如表 8-12 所示。
表 8-12。
Entries for distances to nodes from Node A
| 结节 | 从节点 A 到节点的距离 | 访问 | | --- | --- | --- | | B | five | 是 | | C | Two | 是 | | D | three | 是 | | E | eight | 是 | | F | nine | 是 | | G | Nineteen | 不 | | H | Twenty-two | 不 | | 我 | Sixteen | 是 | | J | nine | 是 |表 8-12 告诉我们下一个要访问的节点是节点 g。然后你把路径加起来，把节点标记为已访问。检查路径时，从节点 G 可访问的唯一其他节点是节点 H。从节点 A 到节点 H 的距离是 36，大于表中所示的 22。因此，表没有更新，节点 G 被标记为已访问。现在已经访问了所有节点，搜索将终止。最终，您将看到表 8-13 ，该表显示了图 8-1 所示图形中从节点 A 到所有其他节点的最短距离。
表 8-13。
Entries for distances to nodes from Node A
| 点头 | 从节点 A 到节点的距离 | 访问 | | --- | --- | --- | | B | five | 是 | | C | Two | 是 | | D | three | 是 | | E | eight | 是 | | F | nine | 是 | | G | Nineteen | 是 | | H | Twenty-two | 是 | | 我 | Sixteen | 是 | | J | nine | 是 |使用图 8-1 所示的图形，下面的程序使用 Dijkstra 算法寻找从起始节点 A 到所有其他节点的最短路径。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3ch8p1_main.java
//Driver class to set up map using ch8p1_edge, find the shortest //path from
//starting node A to all the other nodes in a given graph.
//******************************************************************
import java.util.ArrayList;
public class ch8p1_main {
static ArrayList<ch8p1_edge> graph = null;
static ch8p1_edge[] parents = null;
static ArrayList<String> unsolvedConn = null;
static ArrayList<String> solvedConn = null;
private static ch8p1_minpath finalPath;
public static void main(String[] args) {
// initialize the nodes set
String[] nodes = { "A", "B", "C", "D", "E", "F", "G", "H", "I", "J" };
// initialize the map with the nodes
graph = new ArrayList<ch8p1_edge>();
graph.add(new ch8p1_edge("A", "B", 7));
graph.add(new ch8p1_edge("A", "C", 2));
graph.add(new ch8p1_edge("A", "D", 3));
graph.add(new ch8p1_edge("A", "E", 17));
graph.add(new ch8p1_edge("B", "F", 4));
graph.add(new ch8p1_edge("C", "B", 3));
graph.add(new ch8p1_edge("C", "G", 29));
graph.add(new ch8p1_edge("C", "J", 7));
graph.add(new ch8p1_edge("D", "J", 8));
graph.add(new ch8p1_edge("D", "I", 13));
graph.add(new ch8p1_edge("D", "E", 5));
graph.add(new ch8p1_edge("E", "I", 19));
graph.add(new ch8p1_edge("F", "G", 10));
graph.add(new ch8p1_edge("G", "H", 17));
graph.add(new ch8p1_edge("J", "H", 18));
graph.add(new ch8p1_edge("I", "H", 6));
// initialize the unsolved nodes
unsolvedConn = new ArrayList<String>();
// sets the parent node in the unsolved connection
// ArrayList to A
unsolvedConn.add(nodes[0]);
// initialize the solved nodes
solvedConn = new ArrayList<String>();
// Add all nodes to the solved connection tree
for (int i = 1; i < nodes.length; i++)
solvedConn.add(nodes[i]);
// create a parent array that will store all the edges
parents = new ch8p1_edge[nodes.length];
// Set the initial node to A and make its parent null // with a weight cost
// of zero
parents[0] = new ch8p1_edge(null, nodes[0], 0);
for (int i = 0; i < solvedConn.size(); i++) {
// get all of the String node names that could be // attached the
// root
String n = solvedConn.get(i);
// Check the weights of all the nodes that are
// attached to the root
// A node
// If they are attached will return positive
// weight if not will
// return -1
parents[i + 1] = new ch8p1_edge(nodes[0], n, getEdgeLength(
nodes[0], n));
}
finalPath = null;
// while the solved nodes ArrayList is greater than
// zero
while (solvedConn.size() > 0) {
// Create a minimum shortest path object to find // the shortest path
// to all connected points
ch8p1_minpath msp = getMinSideNode();
finalPath = msp;
if (msp.getEdgeLength() == -1)
msp.outputPath(nodes[0]);
else
msp.outputPath();
String node = msp.getLastNode();
unsolvedConn.add(node);
setEdgeLength(node);
}
}
public static String getParent(ch8p1_edge[] parents, String node) {
if (parents != null) {
for (ch8p1_edge nd : parents) {
if (nd.getChildNode() == node) {
return nd.getParentNode();
}
}
}
return null;
}
public static void setEdgeLength(String parentNode) {
if (graph != null && parents != null && solvedConn != null) {
for (String node : solvedConn) {
ch8p1_minpath msp = getMinPath(node);
int w1 = msp.getEdgeLength();
if (w1 == -1)
continue;
for (ch8p1_edge n : parents) {
if (n.getChildNode() == node) {
if (n.getEdgeLength() == -1 || n.getEdgeLength() > w1) {
n.setEdgeLength(w1);
n.setParentNode(parentNode);
break;
}
}
}
}
}
}
public static int getEdgeLength(String parentNode, String childNode) {
if (graph != null) {
for (ch8p1_edge s : graph) {
if (s.getParentNode() == parentNode
&& s.getChildNode() == childNode)
return s.getEdgeLength();
}
}
return -1;
}
public static ch8p1_minpath getMinSideNode() {
// Create a minimum shortest path object
ch8p1_minpath minMsp = null;
// while the solved node ArrayList is greater than zero
if (solvedConn.size() > 0) {
// Create an index value set to zero
int index = 0;
// for each value in the solved nodes ArrayList
for (int j = 0; j < solvedConn.size(); j++) {
// Create a shortest path map get the
// MinPath of the solved node
ch8p1_minpath msp = getMinPath(solvedConn.get(j));
// if there is no minimum shortest path, // if the minimum shortest
// path
// does not equal -1
// AND the minimum shortest path get
// weight is less than the
// minimum shortest path get weight
if (minMsp == null || msp.getEdgeLength() != -1
&& msp.getEdgeLength() < minMsp.getEdgeLength()) {
// set the minimum shortest path to // the minimum shortest
// path
minMsp = msp;
// set the index value equal it the // node value j
index = j;
}
}
// remove the index that you checked in the
// solved nodes
solvedConn.remove(index);
}
// return the MinShortPath object
return minMsp;
}
public static ch8p1_minpath getMinPath(String node) {
// Create a Minshort Path object that is an ArrayList // and set the take
// in node as the base
// in this case will always be zero
ch8p1_minpath msp = new ch8p1_minpath(node);
// if the parents array does not equal null and the
// unsolved nodes does
// not equal null
if (parents != null && unsolvedConn != null) {
for (int i = 0; i < unsolvedConn.size(); i++) {
ch8p1_minpath tempMsp = new ch8p1_minpath(node);
String parent = unsolvedConn.get(i);
String curNode = node;
while (parent != null) {
int weight = getEdgeLength(parent, curNode);
if (weight > -1) {
tempMsp.addNode(parent);
tempMsp.addEdgeLength(weight);
curNode = parent;
parent = getParent(parents, parent);
} else
break;
}
if (msp.getEdgeLength() == -1 || tempMsp.getEdgeLength() != -1
&& msp.getEdgeLength() > tempMsp.getEdgeLength())
msp = tempMsp;
}
}
return msp;
}
}
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch8p1_minpath.java
//the minimum path from starting node to all the other nodes, //including each node
//and the corresponding minimum distance as well
//******************************************************************************************
import java.util.ArrayList;
public class ch8p1_minpath {
private ArrayList<String> nodeList;
private int edgeLength;
public ch8p1_minpath(String node) {
nodeList = new ArrayList<String>();
nodeList.add(node);
edgeLength = -1;
}
public ArrayList<String> getNodeList() {
return nodeList;
}
public void setNodeList(ArrayList<String> nodeList) {
this.nodeList = nodeList;
}
public void addNode(String node) {
if (nodeList == null)
nodeList = new ArrayList<String>();
nodeList.add(0, node);
}
public String getLastNode() {
int size = nodeList.size();
return nodeList.get(size - 1);
}
public int getEdgeLength() {
return edgeLength;
}
public void setEdgeLength(int edgeLength) {
this.edgeLength = edgeLength;
}
public void outputPath() {
outputPath(null);
}
public void outputPath(String pathNode) {
String result = "the minimum path of ";
if (pathNode != null)
nodeList.add(pathNode);
for (int i = 0; i < nodeList.size(); i++) {
result += "" + nodeList.get(i);
if (i < nodeList.size() - 1)
result += "->";
}
result += " is:" + edgeLength;
System.out.println(result);
}
public void addEdgeLength(int e) {
if (edgeLength == -1)
edgeLength = e;
else
edgeLength += e;
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch8p1_edge.java
//edge class including the parent node and child node
//and the edge length between two nodes
//******************************************************************
public class ch8p1_edge {
private String parentNode;
private String childNode;
private int edgeLength;
public ch8p1_edge(String parentNode, String childNode, int edgeLength) {
this.parentNode = parentNode;
this.childNode = childNode;
this.edgeLength = edgeLength;
}
public String getParentNode() {
return parentNode;
}
public void setParentNode(String parentNode) {
this.parentNode = parentNode;
}
public String getChildNode() {
return childNode;
}
public void setChildNode(String childNode) {
this.childNode = childNode;
}
public int getEdgeLength() {
return edgeLength;
}
public void setEdgeLength(int edgeLength) {
this.edgeLength = edgeLength;
}
}
运行程序的结果如下:
The minimum path of A->C is: 2
The minimum path of A->D is: 3
The minimum path of A->C->B is: 5
The minimum path of A->D->E is: 8
The minimum path of A->C->B->F is: 9
The minimum path of A->C->J is: 9
The minimum path of A->D->I is:16
The minimum path of A->C->B->F->G is: 19
The minimum path of A->D->I->H is: 22
如您所见，这个结果与表 8-13 中显示的结果完全相同。
基于莱霍斯·EV3 的迪杰斯特拉算法
参考图 8-5 ，你将为你的机器人编写一个程序，使它能够使用 Dijkstra 算法在起始节点 A 和目的节点 I 之间行进尽可能短的距离。假设您使用笛卡尔坐标系:
- A 处的坐标是(0，0)。
- B 点的坐标是(-10，20)。
- C 点的坐标是(0，20)。
- D 点的坐标是(-15，20)。
- E 点的坐标是(-15，30)。
- F 点的坐标是(0，30)。
- 在 G 点的坐标是(10，10)。
- H 点的坐标是(10，20)。
- I 处的坐标是(10，30)。
图 8-5。
Graphic to be used for conducting the shortest-path search
您的程序应至少在 LCD 上显示目的地的坐标，然后显示消息“Press ENTER Key to continue”当你按下回车键，你的机器人移动到下一个节点。例如，假设你的机器人从Node A(0,0)出发，你想让它探索一条通往Node G(10,10)的道路。假设您的机器人使用 Dijkstra 算法探索的最小距离路径是Node A -> Node C -> Node G。在起点节点 A，您的程序应该执行以下操作:
- 在 LCD 上显示目的地的坐标
Node C(0, 20)。 - 显示信息“
Press ENTER key to continue.” - 转到坐标为 0，20 的位置。
- 在 LCD 上显示目的地的坐标
Node G(10, 10)。 - 显示信息“
Press ENTER key to continue.” - 前往坐标为 10，10 的位置。
此外，您的问题应该有一个名为 destination 的字符串，这样当更改 destination 的值时，它就足够智能，您的机器人可以探索一条从起始节点 A 到新的目的节点的新路径，在这条路径上，机器人将以最小的距离到达目的节点。(我们假设起始节点总是 A，所以 from 字符串可以硬编码。)
以下程序表示基于 leJOS 的 Dijkstra 算法的实现，该算法被设计成探索从节点 A 到目的节点 I 的最短路径:
//******************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch8p2_main.java
//Driver class to set up map using ch8p2_edge, find the shortest //path from
//starting node A to all the other nodes in a given graph.
//and then robots will follow the path to move from start node A
//to the given destination node I
//******************************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.robotics.chassis.Chassis;
import lejos.robotics.chassis.Wheel;
import lejos.robotics.chassis.WheeledChassis;
import lejos.robotics.navigation.MovePilot;
import lejos.robotics.navigation.Navigator;
import lejos.robotics.navigation.Waypoint;
import lejos.robotics.pathfinding.Path;
import java.util.ArrayList;
public class ch8p2_main {
static ArrayList<ch8p2_edge> graph = null;
static ch8p2_edge[] parents = null;
static ArrayList<String> unsolvedConn = null;
static ArrayList<String> solvedConn = null;
private static ch8p2_minpath finalPath;
static EV3LargeRegulatedMotor LEFT_MOTOR = new EV3LargeRegulatedMotor(
MotorPort.A);
static EV3LargeRegulatedMotor RIGHT_MOTOR = new EV3LargeRegulatedMotor(
MotorPort.C);
static Waypoint[] coordinates = { new Waypoint(0, 0),
new Waypoint(-10, 20), new Waypoint(0, 20), new Waypoint(-15, 20),
new Waypoint(-15, 30), new Waypoint(0, 30), new Waypoint(10, 10),
new Waypoint(10, 20), new Waypoint(10, 30),
};
public static void main(String[] args) {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// instantiated LCD class for displaying and Keys class // for buttons
Keys buttons = ev3brick.getKeys();
// setup the wheel diameter of left (and right) motor // in centimeters,
// i.e. 2.8 cm
// the offset number is the distance between the center // of wheel to
// the center of robot, i.e. half of track width
Wheel wheel1 = WheeledChassis.modelWheel(LEFT_MOTOR, 2.8).offset(-9);
Wheel wheel2 = WheeledChassis.modelWheel(RIGHT_MOTOR, 2.8).offset(9);
// set up the chassis type, i.e. Differential pilot
Chassis chassis = new WheeledChassis(new Wheel[] { wheel1, wheel2 },
WheeledChassis.TYPE_DIFFERENTIAL);
MovePilot ev3robot = new MovePilot(chassis);
Navigator navbot = new Navigator(ev3robot);
// initialize the nodes set
String[] nodes = { "A", "B", "C", "D", "E", "F", "G", "H", "I" };
// initialize the map with the nodes
graph = new ArrayList<ch8p2_edge>();
graph.add(new ch8p2_edge("A", "B", 22));
graph.add(new ch8p2_edge("A", "C", 20));
graph.add(new ch8p2_edge("A", "D", 25));
graph.add(new ch8p2_edge("B", "C", 10));
graph.add(new ch8p2_edge("C", "E", 18));
graph.add(new ch8p2_edge("C", "F", 10));
graph.add(new ch8p2_edge("C", "G", 14));
graph.add(new ch8p2_edge("C", "H", 10));
graph.add(new ch8p2_edge("C", "I", 18));
graph.add(new ch8p2_edge("D", "B", 5));
graph.add(new ch8p2_edge("D", "E", 10));
graph.add(new ch8p2_edge("D", "F", 18));
graph.add(new ch8p2_edge("E", "F", 15));
graph.add(new ch8p2_edge("F", "I", 10));
graph.add(new ch8p2_edge("G", "H", 10));
graph.add(new ch8p2_edge("H", "F", 14));
graph.add(new ch8p2_edge("H", "I", 10));
// initialize the unsolved nodes
unsolvedConn = new ArrayList<String>();
// sets the parent node in the unsolved connection
// ArrayList to A
unsolvedConn.add(nodes[0]);
// initialize the solved nodes
solvedConn = new ArrayList<String>();
// Add all nodes to the solved connection tree
for (int i = 1; i < nodes.length; i++)
solvedConn.add(nodes[i]);
// create a parent array that will store all the edges
parents = new ch8p2_edge[nodes.length];
// Set the initial node to A and make its parent null // with a weight cost
// of zero
parents[0] = new ch8p2_edge(null, nodes[0], 0);
for (int i = 0; i < solvedConn.size(); i++) {
// get all of the String node names that could be // attached the
// root
String n = solvedConn.get(i);
// Check the weights of all the nodes that are
// attached to the root
// A node
// If they are attached will return positive
// weight if not will
// return -1
parents[i + 1] = new ch8p2_edge(nodes[0], n, getEdgeLength(
nodes[0], n));
}
finalPath = null;
// while the solved nodes ArrayList is greater than zero
while (solvedConn.size() > 0) {
// Create a minimum shortest path object to find // the shortest path
// to all connected points
ch8p2_minpath msp = getMinSideNode();
finalPath = msp;
String node = msp.getLastNode();
unsolvedConn.add(node);
setEdgeLength(node);
}
Path directions = finalPath.buildPath(coordinates);
navbot.setPath(directions);
navbot.singleStep(true);
while (navbot.getWaypoint() != null) {
if (navbot.getWaypoint() != null) {
System.out.println("Next destination" + navbot.getWaypoint());
}
System.out.println("Press Enter Key to Continue");
// block the thread until a button is pressed
buttons.waitForAnyPress();
navbot.followPath();
while (navbot.isMoving())
;
}
// block the thread until a button is pressed
buttons.waitForAnyPress();
}
public static String getParent(ch8p2_edge[] parents, String node) {
if (parents != null) {
for (ch8p2_edge nd : parents) {
if (nd.getChildNode() == node) {
return nd.getParentNode();
}
}
}
return null;
}
public static void setEdgeLength(String parentNode) {
if (graph != null && parents != null && solvedConn != null) {
for (String node : solvedConn) {
ch8p2_minpath msp = getMinPath(node);
int w1 = msp.getEdgeLength();
if (w1 == -1)
continue;
for (ch8p2_edge n : parents) {
if (n.getChildNode() == node) {
if (n.getEdgeLength() == -1 || n.getEdgeLength() > w1) {
n.setEdgeLength(w1);
n.setParentNode(parentNode);
break;
}
}
}
}
}
}
public static int getEdgeLength(String parentNode, String childNode) {
if (graph != null) {
for (ch8p2_edge s : graph) {
if (s.getParentNode() == parentNode
&& s.getChildNode() == childNode)
return s.getEdgeLength();
}
}
return -1;
}
public static ch8p2_minpath getMinSideNode() {
// Create a minimum shortest path object
ch8p2_minpath minMsp = null;
// while the solved node ArrayList is greater than zero
if (solvedConn.size() > 0) {
// Create an index value set to zero
int index = 0;
// for each value in the solved nodes ArrayList
for (int j = 0; j < solvedConn.size(); j++) {
// Create a shortest path map get the
// MinPath of the solved node
ch8p2_minpath msp = getMinPath(solvedConn.get(j));
// if there is no minimum shortest path, // if the minimum shortest
// path
// does not equal -1
// AND the minimum shortest path get
// weight is less than the
// minimum shortest path get weight
if (minMsp == null || msp.getEdgeLength() != -1
&& msp.getEdgeLength() < minMsp.getEdgeLength()) {
// set the minimum shortest path to // the minimum shortest
// path
minMsp = msp;
// set the index value equal it the // node value j
index = j;
}
}
// remove the index that you checked in the
// solved nodes
solvedConn.remove(index);
}
// return the MinShortPath object
return minMsp;
}
public static ch8p2_minpath getMinPath(String node) {
// Create a Minshort Path object that is an ArrayList // and set the take
// in node as the base
// in this case will always be zero
ch8p2_minpath msp = new ch8p2_minpath(node);
// if the parents array does not equal null and the
// unsolved nodes does
// not equal null
if (parents != null && unsolvedConn != null) {
for (int i = 0; i < unsolvedConn.size(); i++) {
ch8p2_minpath tempMsp = new ch8p2_minpath(node);
String parent = unsolvedConn.get(i);
String curNode = node;
while (parent != null) {
int weight = getEdgeLength(parent, curNode);
if (weight > -1) {
tempMsp.addNode(parent);
tempMsp.addEdgeLength(weight);
curNode = parent;
parent = getParent(parents, parent);
} else
break;
}
if (msp.getEdgeLength() == -1 || tempMsp.getEdgeLength() != -1
&& msp.getEdgeLength() > tempMsp.getEdgeLength())
msp = tempMsp;
}
}
return msp;
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch8p2_edge.java
//edge class including the parent node and child node
//and the edge length between two nodes
//******************************************************************
public class ch8p2_edge {
private String parentNode;
private String childNode;
private int edgeLength;
public ch8p2_edge(String parentNode, String childNode, int edgeLength) {
this.parentNode = parentNode;
this.childNode = childNode;
this.edgeLength = edgeLength;
}
public String getParentNode() {
return parentNode;
}
public void setParentNode(String parentNode) {
this.parentNode = parentNode;
}
public String getChildNode() {
return childNode;
}
public void setChildNode(String childNode) {
this.childNode = childNode;
}
public int getEdgeLength() {
return edgeLength;
}
public void setEdgeLength(int edgeLength) {
this.edgeLength = edgeLength;
}
}
//************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch8p2_minpath.java
//the minimum path from starting node to all the other nodes, //including each node
//and the corresponding minimum distance as well
//************************************************************************************
import java.util.ArrayList;
import lejos.robotics.navigation.Waypoint;
import lejos.robotics.pathfinding.Path;
public class ch8p2_minpath {
private ArrayList<String> nodeList;
private int edgeLength;
public ch8p2_minpath(String node) {
nodeList = new ArrayList<String>();
nodeList.add(node);
edgeLength = -1;
}
public ArrayList<String> getNodeList() {
return nodeList;
}
public void setNodeList(ArrayList<String> nodeList) {
this.nodeList = nodeList;
}
public void addNode(String node) {
if (nodeList == null)
nodeList = new ArrayList<String>();
nodeList.add(0, node);
}
public String getLastNode() {
int size = nodeList.size();
return nodeList.get(size - 1);
}
public int getEdgeLength() {
return edgeLength;
}
public void setEdgeLength(int edgeLength) {
this.edgeLength = edgeLength;
}
public void outputPath() {
outputPath(null);
}
public Path buildPath(Waypoint[] coordinates) {
Path result = new Path();
int p = 0;
for (int i = 0; i < nodeList.size(); i++) {
if (nodeList.get(i).equals("A"))
p = 0;
else if (nodeList.get(i).equals("A"))
p = 0;
else if (nodeList.get(i).equals("B"))
p = 1;
else if (nodeList.get(i).equals("C"))
p = 2;
else if (nodeList.get(i).equals("D"))
p = 3;
else if (nodeList.get(i).equals("E"))
p = 4;
else if (nodeList.get(i).equals("F"))
p = 5;
else if (nodeList.get(i).equals("G"))
p = 6;
else if (nodeList.get(i).equals("H"))
p = 7;
else if (nodeList.get(i).equals("I"))
p = 8;
result.add(coordinates[p]);
}
return result;
}
public void outputPath(String pathNode) {
String result = "the munimum path of ";
if (pathNode != null)
nodeList.add(pathNode);
for (int i = 0; i < nodeList.size(); i++) {
result += "" + nodeList.get(i);
if (i < nodeList.size() - 1)
result += "->";
}
result += " is:" + edgeLength;
System.out.println(result);
}
public void addEdgeLength(int e) {
if (edgeLength == -1)
edgeLength = e;
else
edgeLength += e;
}
}
摘要
本章介绍了 Dijkstra 搜索算法背后的基本思想和基本原理。你现在能够在实践中应用 Dijkstra 算法来解决路径规划问题。特别是，本章介绍了如何基于 Dijkstra 的路径规划算法和导航类来构建一个问题解决智能体，在该导航类中，问题解决智能体可以智能地找到从起点到任何目的地的路线路径，覆盖尽可能最短的距离。
在下一章中，您将看到如何基于 A* 路径规划算法和导航类构建一个问题解决代理，其中问题解决代理可以智能地在迷宫中找到从起点到任何目的地的路线路径。
九、A* 搜索算法及其在 Lego Mindstorms 中的实现
本章将向你介绍 A* 搜索算法的基础。学完本章后，你将能够在实践中应用 A* 算法解决路径规划问题。您还将看到如何基于 A* 路径规划算法和导航类来构建问题解决代理，其中问题解决代理可以在迷宫中智能地找到从起点到任何目的地的路线路径。
特别是，本章将涵盖以下主题:
- 什么是 A* 算法？
- A* 搜索策略的基本思想。
- 使用 A* 算法进行路径规划的编程实践。
什么是 A* 算法？
A-Star(或 A* )是一种众所周知的搜索算法，与其他搜索算法相比，它在解决路径发现的效率方面极具竞争力。特别是，A* 算法最适合用于那些可以表示为状态空间的问题，例如，在迷宫中探索路径。给定一个合适的问题，问题的初始条件可以用合适的初始状态来表示，目标条件可以表示为目标状态。
对于您执行的每个操作，A* 算法都会生成后续状态来表示该操作的结果。如果你一直这样做，并且在某个时刻生成的后续状态之一是目标状态，那么从初始状态到目标状态的路径就是你的问题的解决方案。而且 A* 算法以一定的方式生成和处理后继状态；也就是说，每当它寻找下一个状态继续时，A* 算法采用启发式函数来尝试挑选下一个要处理的最佳状态。
A* 搜索策略的基本思想
如图 9-1 所示，假设你要从节点 1 到节点 5，有一堵墙把节点 1 和节点 5 隔开。搜索区域被分割成一个基于正方形的网格，您将试图找到的路径是从节点 1 到节点 5 要走哪些正方形。在整个搜索区域中有 15 个方块，标记为 1 到 15。左上角是起点 1，右上角是终点 5，节点 3 和 8 位于中心来表示墙。
图 9-1。
The search area of the A* Algorithm
一旦将搜索区域简化为预定义数量的节点，下一步就是进行搜索，以找到从起点节点 1 到终点节点 5 的最短路径，这是最终目标。从节点 1 开始，检查其相邻的方块，然后通常向外搜索，直到找到目标节点 5。
通过执行以下操作开始 A* 搜索:
- 从起点 1 开始，将其添加到一个“开放列表”中，该列表包含沿着您想要选择的路径的方块。基本上，这是一个需要检查的方块列表。比如
OpenList = {1}。 - 查看与起点 1 相邻的所有可到达的正方形，忽略有墙的正方形，并将它们添加到开放列表中。对于这些方块中的每一个，将节点 1 保存为其“父方块”:即
OpenList = {1,2,6}。 - 从开放列表中删除起点方块 1，然后将其添加到方块的“封闭列表”中，现在您不需要再查看它。于是就有了
OpenList = {2,6}和ClosedList = {1}。 - 所有相邻的方块现在都在要检查的方块的开放列表中。
接下来你需要在开放列表中选择一个相邻的方块继续。但是，有一个问题，你选择哪个广场？答案是它是 F 成本最低的一个，其中 F 是路径得分，这是决定使用哪些方块的关键。
F 是 G 和 H 的和(F = G + H ),其中 G 是沿着生成的路径从起始节点 1 移动到网格上的给定方块的成本，H 是从网格上的那个方块移动到最终目的地(即结束节点 5)的估计成本。启发式函数 H 包括通过反复检查你的开放列表并选择 F 分数最低的方块来生成你的路径。
首先让我们更仔细地看看你是如何计算这个方程的。
- F = G + H
g 是使用生成的路径从起始节点 1 移动到给定方块的成本。在本例中，您为移动的每个水平或垂直方块分配 1 的成本。因此，您有以下内容:
- G(1->2)=1
- G(1->6)=1
启发式函数 H 可以用多种方式来估计。在此示例中，使用了曼哈顿方法，您可以计算从当前方块水平和垂直移动到目标方块的方块总数，忽略对角线移动和可能存在的任何障碍。结果，你有了启发式函数 H:
- H(2->5)=3
- H(6->5)=5
f 是 G 和 h 相加计算出来的。
- F(2)=1+3=4
- F(6)=1+5=6
接下来，如图 9-2 所示，你只需从开放列表的现有节点中选择最低 F 值的方块，即节点 2。然后从开放列表中删除节点 2，并将其添加到封闭列表中，即OpenList = {6}和ClosedList = {1,2}。
图 9-2。
Search from Node 1 to Node 2
检查节点 2 的所有相邻方块。忽略那些在封闭列表中的或不可达的(节点 3 不可达，因为它属于墙)，如果方块不在这个列表中，将它们添加到开放列表中。使所选方块成为新方块的“父方块”。因此，您会得到以下内容:
-
OpenList = {6,7}in which 6’s parent is Node 1 and 7’s parent is Node 2, as shown in Figure 9-3.
图 9-3。
Tree structure of nodes after visiting Node 2
-
ClosedList = {1,2}
现在开放列表中有两个节点(节点 6 和节点 7)。然后计算 F 的值，然后选择 F 值最低的一个。
OpenList = {6,7}ClosedList = {1,2}
在这种情况下，节点 6 和节点 7 具有相同的 F 值 6。那么你选择哪个呢？出于速度的考虑，选择 H 值较小的节点(即节点 7)可能会更快。然后你只需从开放列表上的现有节点中选择最低的 F 得分方块，即节点 7，并将节点 7 从开放列表中删除并将其添加到封闭列表中，即OpenList = {6} and ClosedList = {1,2,7}。
接下来，检查节点 7 的所有相邻方块。忽略那些在封闭列表上或不可达的方块(节点 8 不可达)，如果方块还不在这个列表上，就将它们添加到开放列表中。将选中的方块作为新方块的“父方块”，如图 9-4 所示。因此，您会得到以下内容:
图 9-4。
Tree structure of nodes after visiting Node 7
OpenList = {6,12}ClosedList = {1,2,7}
在这种情况下，节点 6 已经在开放列表中。然后，您需要检查到节点 6 的当前路径是否是更好的路径。使用当前路径1->2->7，您可以计算节点 6 的当前 G 值G(1->2->7->6)，这是 3，然后将其与之前的 G 值G(1->6)，这是 1 进行比较。如果当前的 G 值低于前一个值，则将节点 6 的父节点更改为节点 7。如果不低于上一条，就不做单子上的事。由于先前的 G 分数(1->6)小于当前的 G 分数(1->2->7->6)，所以你不必做列表上的任何事情。
接下来，计算节点 6 和节点 12 的函数 F:
- 你已经知道了
F(6) = 6 H(12->5)=5G(1->12)=3
这样你就有了F(12) = 8，它比F(6)大。然后选择节点 6 作为下一个节点，您可以从开放列表中删除节点 6，并将其添加到封闭列表中，如下所示:
OpenList = {12}ClosedList = {1,2,7,6}
节点 6 的父节点是 1，如图 9-5 所示。
图 9-5。
Tree structure of nodes in which Node 6 is chosen as the next node
检查节点 6 的所有相邻方块。忽略那些在封闭列表中或不可及的方块，如果它们不在这个列表中，将它们添加到开放列表中。将选中的方块作为新方块的“父方块”，如图 9-6 所示。因此，您会得到以下内容:
图 9-6。
Tree structure of nodes after visiting Node 6
OpenList = {12,11}ClosedList = {1,2,7,6}
接下来，计算节点 11 和节点 12 的函数 F:
- 你已经知道了
F(12) = 8 H(11->5)=6G(1->11)=2
于是就有了F(12) = 8，和F(11)一样。那么你选择哪个呢？类似地，出于速度的目的，选择具有较小 H 分数的节点(即节点 12)可能会更快。
然后选择节点 12 作为下一个节点，从开放列表中删除节点 12，并将其添加到封闭列表中，这样就有了以下内容:
OpenList = {11}ClosedList = {1,2,7,6,12}
节点 12 的父节点是 7，如图 9-7 所示。
图 9-7。
Tree structure of nodes in which Node 12 is chosen as the next node
检查节点 12 的所有相邻方块。忽略那些在封闭列表中或不可及的方块，如果它们不在这个列表中，将它们添加到开放列表中。将选中的方块作为新方块的“父方块”，如图 9-8 所示。
图 9-8。
Tree structure of nodes after visiting Node 12
OpenList = {11,13}ClosedList = {1,2,7,6,12}
接下来，计算节点 11 和节点 13 的函数 F:
- 你已经知道了
F(11) = 8 H(13->5)=4G(1->13)=4
于是就有了F(13) = 8，和F(11)一样。那么你选择哪个呢？类似地，出于速度的目的，选择具有较小 H 分数的节点(即节点 13)可能会更快。
然后选择节点 13 作为下一个节点。您从开放列表中删除节点 13，并将其添加到封闭列表中，如下所示:
OpenList = {11}ClosedList = {1,2,7,6,12,13}
节点 13 的父节点是 12，如图 9-9 所示。
图 9-9。
Tree structure of nodes in which Node 13 is chosen as the next node
检查节点 13 的所有相邻方块。忽略那些在封闭列表中或不可及的方块，如果它们不在这个列表中，将它们添加到开放列表中。将选中的方块作为新方块的“父方块”，如图 9-10 所示。
OpenList = {11,14}ClosedList = {1,2,7,6,12,13}
图 9-10。
Tree structure of nodes after visiting Node 13
接下来，计算节点 11 和节点 14 的函数 F:
- 你已经知道了
F(11)=8 H(14->5)=5G(1->14)=3
于是你就有了F(14) = 8，和F(11)一样。那么你选择哪个呢？类似地，出于速度的目的，选择具有较小 H 分数的节点(即节点 14)可能会更快。
然后选择节点 14 作为下一个节点。您从开放列表中删除节点 14，并将其添加到封闭列表中，如下所示:
OpenList = {11}ClosedList = {1,2,7,6,12,13,14}
节点 14 的父节点是 13。
检查节点 14 的所有相邻方块。忽略那些在封闭列表中或无法到达的方块，如果方块 9 和 15 不在列表中，将它们添加到开放列表中。使所选方块成为新方块的“父方块”。
OpenList = {11,9,15}ClosedList = {1,2,7,6,12,13,14}
接下来，计算节点 11、节点 9 和节点 15 的函数 F。
- 你已经知道了
F(11) = 8 - 按照我们之前的思路，可以算出
F(9) = 8和F(15) = 8，和F(11)一样。
你选择哪个？类似地，出于速度的目的，选择具有较小 H 分数的那个可以更快。然后选择节点 15 作为下一个节点，从开放列表中删除节点 15，并将其添加到封闭列表中，如下所示:
OpenList = {11,9}- 15 的父母是 14
检查节点 15 的所有相邻方块。忽略那些在封闭列表中或不可及的方块，如果它们不在这个列表中，将它们添加到开放列表中。使所选方块成为新方块的“父方块”。
OpenList = {11,9,10}ClosedList = {1,2,7,6,12,13,14,15}
接下来，计算节点 11、节点 9 和节点 10 的函数 F。
- 你有
F(11)=F(9)=F(10)=8
类似地，出于速度的目的，选择具有较小 H 分数的节点(即节点 10)可能会更快。然后选择节点 10 作为下一个节点，从开放列表中删除节点 10，并将其添加到封闭列表中，如下所示:
OpenList = {11,9,5}- 10 的父代是 15
找到目标状态节点 5，并在那里停止。因此，最短路径位于根据父节点的封闭列表中，如下所示:
1->2->7->12->13->14->15->10->5
使用 A* 算法进行路径规划的实施规程
在迷宫中寻找路径是一个有趣的问题，通过计算机已经很大程度上解决了这个问题。这个练习的目标是在尽可能短的时间内解决一个简单的迷宫，从一个点开始到另一个点，不管这两点之间有什么障碍。如图 9-11 所示，您的机器人将探索从蓝色圆圈开始到绿色椭圆形结束的路径。平面是一个尺寸为 90 英寸乘 90 英寸的正方形:也就是说，H 和 I 之间的距离是 90 英寸。
图 9-11。
Maze map
从图 9-11 中的地图可以看到:
- A 点和 B 点之间的距离是 10 英寸。
- A 点和 H 点之间的距离是 20 英寸。
- A 点和 I 点之间的距离是 70 英寸。
- B 点和 C 点之间的距离是 50 英寸。
- C 点和 D 点之间的距离是 20 英寸。
- J 点和 E 点之间的距离是 20 英寸。
- E 点和 K 点之间的距离是 40 英寸。
- K 点和 L 点之间的距离是 20 英寸。
- L 点和 M 点之间的距离是 20 英寸。
为了允许各种各样的迷宫解决方法，迷宫的起点和终点将被提前知道，如图 9-11 所示，并且不会有循环。在真实的演示中，您将使用蓝色标记 A，以标识点 A 为起点(在笛卡尔坐标系中，例如，您可以说 A 的坐标是(0,-20))。此外，您将使用绿色来标记线 A 到 H，以便您的机器人可以在到达绿色目的地 B)时停止。在地图中，所有的交叉点都是直角。黑色线条代表墙壁周围。
总而言之，在本练习中，您将创建一个迷宫旅行机器人，它可以探索地图上从蓝色区域开始到绿色区域结束的路径。而且，你的机器人可以检测到终点区域的绿色线，然后在找到最终目的地时通过识别它停下来。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0 ch9p1.java
//simple line map and grid using A\* search build into LeJOS
//******************************************************************
import lejos.geom.*; //used for rectangle
import lejos.robotics.RegulatedMotor; //motor controller
import lejos.robotics.localization.*; //numbers
import lejos.robotics.mapping.LineMap; //mapping
import lejos.robotics.navigation.*; //navigation used for the
// waypoints
import lejos.robotics.pathfinding.*; //A\* search algorithm
import lejos.util.PilotProps; //not used really
import lejos.nxt.Sound;
import lejos.robotics.navigation.DifferentialPilot;
public class ch9p1 {
private static final short[] note = { 2349, (115 / 3), 0, (5 / 3), 1760,
(165 / 3), 0, (35 / 3), 1760, (28 / 3), 0, (13 / 3), 1976,
(23 / 3), 0, (18 / 3), 1760, (18 / 3), 0, (23 / 3), 1568, (15 / 3),
0, (25 / 3), 1480, (103 / 3), 0, (18 / 3), 1175, (180 / 3), 0,
(20 / 3), 1760, (18 / 3), 0, (23 / 3), 1976, (20 / 3), 0, (20 / 3),
1760, (15 / 3), 0, (25 / 3), 1568, (15 / 3), 0, (25 / 3), 2217,
(98 / 3), 0, (23 / 3), 1760, (88 / 3), 0, (33 / 3), 1760, (75 / 3),
0, (5 / 3), 1760, (20 / 3), 0, (20 / 3), 1760, (20 / 3), 0,
(20 / 3), 1976, (18 / 3), 0, (23 / 3), 1760, (18 / 3), 0, (23 / 3),
2217, (225 / 3), 0, (15 / 3), 2217, (218 / 3) };
static RegulatedMotor leftMotor; // motors
static RegulatedMotor rightMotor;
public static void main(String[] args) {
// set up the robot
PilotProps pp = new PilotProps();
float wheelDiameter = Float.parseFloat(pp.getProperty(
PilotProps.KEY_WHEELDIAMETER, "2.11"));
float trackWidth = Float.parseFloat(pp.getProperty(
PilotProps.KEY_TRACKWIDTH, "5.45"));
RegulatedMotor leftMotor = PilotProps.getMotor(pp.getProperty(
PilotProps.KEY_LEFTMOTOR, "C"));
RegulatedMotor rightMotor = PilotProps.getMotor(pp.getProperty(
PilotProps.KEY_RIGHTMOTOR, "A"));
leftMotor.setSpeed(750);
rightMotor.setSpeed(750);
leftMotor.setAcceleration(1000);
rightMotor.setAcceleration(1000);
boolean reverse = Boolean.parseBoolean(pp.getProperty(
PilotProps.KEY_REVERSE, "false"));
// new robot object using the setup
DifferentialPilot robot = new DifferentialPilot(wheelDiameter,
trackWidth, leftMotor, rightMotor, reverse);
// make the robot move faster this is over max
// robot.setTravelSpeed(500);
robot.setRotateSpeed(600);
// Create final map:
Line[] lines = new Line[8]; // six lines inside the map
lines[0] = new Line(-2.5f, -2.5f, -2.5f, 67.5f);
// line AG
lines[1] = new Line(-5.0f, -2.5f, -5.0f, 67.5f);
lines[2] = new Line(-2.5f, 67.5f, 47.5f, 67.5f);
// line GF
lines[3] = new Line(-2.5f, 7.5f, 47.5f, 7.5f);
// line BC
lines[4] = new Line(47.5f, 7.5f, 47.5f, 27.5f);
// line cd
lines[5] = new Line(44f, 7.5f, 44f, 27.5f);
// cd broader
lines[6] = new Line(27.5f, 27.5f, 27.5f, 47.5f);
// line je
lines[7] = new Line(27.5f, 47.5f, 67.5f, 47.5f);
// line ek
lejos.geom.Rectangle bounds = new Rectangle(-22.5f, -2.5f, 90f, 90f);
LineMap myMap = new LineMap(lines, bounds); // add the //bounds to the map
// Use a regular grid of node points. Grid space = 20\. //Clearance = 15:
FourWayGridMesh grid = new FourWayGridMesh(myMap, 10, 2f);
// Use A\* search:
AstarSearchAlgorithm alg = new AstarSearchAlgorithm();
// Give the A\* search alg and grid to the PathFinder:
PathFinder pf = new NodePathFinder(alg, grid);
// store the location of the robot at a given time
PoseProvider posep = new OdometryPoseProvider(robot);
// new navigator loaded with the robot position, and //path
NavPathController nav = new NavPathController(robot, posep, pf);
System.out.println("Planning path…"); // displays as //the path is
calculated
nav.goTo(-12, 0); // goto the end location.
for (int i = 0; i < note.length; i += 2) {
final short w = note[i + 1];
final int n = note[i];
if (n != 0)
Sound.playTone(n, w * 10);
try {
Thread.sleep(w * 10);
} catch (InterruptedException e) {
}
}
}
}
摘要
本章介绍 A* 搜索算法的基本原理。学完本章后，你能够在实践中应用 A* 算法解决路径规划问题。具体来说，本章介绍了如何基于 A* 路径规划算法和导航类构建一个问题求解智能体，其中问题求解智能体智能地找到一条从起点到任意目的地的最短距离的路径。
在下一章中，你将学习如何应用一组传感器来执行有根据的动作。特别是，您将使用您创建的轮式机器人，并添加两个传感器——触摸传感器和超声波传感器——以使它在移动时更加了解周围的环境。
十、触摸传感器和超声波传感器简介
正如您在第三章中所看到的，电机是执行运动的最重要组件。在这一章中，你将学习一组传感器，它们被用来执行有根据的动作。
想象一下，一个机器人在你的房子里走来走去，却没有配备任何传感器。这样的行程将是短暂的，因为机器人撞上了墙壁，而保险杠没有激活或被一些低层障碍物阻挡车轮。然而，如果你在机器人身上安装了一个距离传感器，你可能会想象机器人可以通过使用这个距离传感器避开障碍物，在你的房子周围徘徊几个小时。
乐高 MindStorm EV3/NXT 套件带有四个常见的传感器:触摸传感器，颜色传感器，光传感器和超声波传感器。在本章中，您将使用您创建的轮式机器人，并添加这四个传感器中的两个——触摸传感器和超声波传感器——以使它在房间内移动时更加了解周围的环境。在下一章，你将看到如何应用光传感器和颜色传感器来探索你的机器人周围的环境。
特别是，本章将涵盖以下主题:
- 触摸传感器简介
- 超声波传感器简介
- 触摸传感器编程实践
- 超声波传感器编程实践
传感器类别
莱霍斯 EV3 为所有类型的传感器提供软件抽象。在这种情况下，传感器必须物理连接到 EV3 模块中的端口，并且传感器对象必须知道这是哪个端口。为了识别这些信息，您创建了一个传感器的实例，然后在它的构造函数中传递这些信息，比如SensorPort.S1、SensorPort.S2、SensorPort.S3和SensorPort.S4。在您的代码中，传感器类必须指出它们插入了哪个端口。这可以通过使用端口类lejos.hardware.port.Port来完成。
端口类类似于MotorPort。这个类中有四个静态端口对象。通常，这些在传感器的构造函数中使用，如下所示:
Port portS1 = ev3brick.getPort("S1");
EV3TouchSensor touchSensor = new EV3TouchSensor(portS1);
触摸传感器
触摸传感器是 EV3 机器人套件中最基本的传感器。它有一个简单的开关，由前面的红色按钮激活，如图 10-1 所示:
图 10-1。
The Lego EV3 touch sensor
如图 10-1 所示，触摸按钮有一个轴孔，允许乐高积木轴直接连接到开关上。触摸传感器检测它何时被某物按压，何时被再次释放，它将返回一个简单的浮点值，指示传感器是否被按压。EV3TouchSensor类实现了 Touch 接口，它包含一个简单的方法getTouchMode ()。getTouchMode(方法检测其前端按钮何时被按下，其中样本包含一个元素。值 0 表示按钮未被按下，值 1 表示按钮被按下。
总之，为了使用触摸传感器，您可以使用以下构造函数创建它的一个实例:
EV3TouchSensor touchSensor = new EV3TouchSensor(portS1);
SensorMode toucher = touchSensor.getTouchMode();
float[] sample = new float[toucher.sampleSize()];
要测试触摸传感器是否被按下，您可以使用fetchSample ()方法:
toucher.fetchSample(sample, 0);
if (sample[0]==1) then the button is pressed
以下程序等待触摸传感器被按下。按下触摸传感器后，触摸传感器的值将在 LED 屏幕上显示 50 秒。该计划被终止，直到你按下 EV3 砖上的退出按钮。下面是使用 S1 港的测试程序的源代码。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch10p1.java
//an example for touch sensor testing
//******************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.TextLCD;
import lejos.hardware.port.Port;
import lejos.hardware.sensor.EV3TouchSensor;
import lejos.hardware.sensor.SensorMode;
public class ch10p1 {
public static void main(String[] args) throws Exception {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// LCD class for displaying and Keys class for buttons
Keys buttons = ev3brick.getKeys();
TextLCD lcddisplay = ev3brick.getTextLCD();
// block the thread until a button is pressed
buttons.waitForAnyPress();
// get a port instance
Port portS1 = ev3brick.getPort("S1");
// Get an instance of the touch EV3 sensor
EV3TouchSensor touchSensor = new EV3TouchSensor(portS1);
// get an instance of this sensor in measurement mode
SensorMode toucher = touchSensor.getTouchMode();
// initialize an array of floats for fetching samples
float[] samplevalue = new float[toucher.sampleSize()];
lcddisplay.clear();
while (buttons.getButtons() != Keys.ID_ESCAPE) {
// fetch a sample
toucher.fetchSample(samplevalue, 0);
lcddisplay.drawString("value: " + samplevalue[0], 0, 0);
Thread.sleep(50);
}
lcddisplay.clear();
System.out.println("EXIT");
System.exit(0);
}
}
超声波传感器
超声波传感器发出人类几乎听不见的声音信号，然后测量反射回来需要多长时间。由于音速是众所周知的，传感器可以计算信号传播的距离。超声波传感器通常用于避开障碍物，在不同位置之间导航，甚至在不同位置之间构建地图。超声波传感器以厘米为单位测量到固体物体的距离，它可以测量高达 255 厘米的距离。超声波传感器的精确度从 6 厘米到 180 厘米。经验实验表明，超过 180 厘米的物体不能被可靠地定位。此外，它的精度为正负 3 厘米，尽管对于更近的物体精度会有所提高。
如图 10-2 所示，超声波传感器往往会产生一个声纳锥体，也就是说它在锥体形状内探测前方的物体。圆锥体以大约 30 度的角度打开，这意味着在 180 厘米的距离处，圆锥体的直径大约为 90 厘米。圆锥形对机器人来说很好，因为它可以更好地扫描机器人前面的更大区域，以发现可能的碰撞。
图 10-2。
The Lego EV3 ultrasonic sensor
在 leJOS NXJ 中，超声波传感器是实现测距仪接口的距离传感器。以下方法用于通过测距仪接口获得距离。
double distance = rf.getRange();
这个方法将以厘米为单位返回到一个物体的距离。当单次扫描检测到多个物体时，可使用以下方法获得相应的距离:
float [] distances = rf.getRanges();
若要创建实例，可以使用以下构造函数:
UltrasonicSensor(Port SensorPort)
在 leJOS NXJ 中，超声波传感器以两种模式工作:默认为连续模式和 ping 模式。在 ping 模式下，传感器只发送一次 ping。当在连续模式下运行时，传感器尽可能频繁地发出 pings，最近获得的结果可通过调用以下命令获得:
int getDistance()
返回值以厘米为单位。如果没有找到回应，返回值将是 255。
以下程序将测试您的超声波传感器是否工作正常；也就是说，它能够识别你的机器人和它前面的障碍物之间的厘米距离。在程序中，我们使用端口号 S1，并在按下退出键后终止程序。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch10p2.java
//an example for ultrasonic sensor testing
//******************************************************************
import lejos.hardware.BrickFinder;
import lejos.hardware.Keys;
import lejos.hardware.ev3.EV3;
import lejos.hardware.lcd.TextLCD;
import lejos.hardware.port.Port;
import lejos.hardware.sensor.EV3UltrasonicSensor;
import lejos.robotics.SampleProvider;
public class ch10p2 {
public static void main(String[] args) throws Exception {
// get EV3 brick
EV3 ev3brick = (EV3) BrickFinder.getLocal();
// LCD class for displaying and Keys class for buttons
Keys buttons = ev3brick.getKeys();
TextLCD lcddisplay = ev3brick.getTextLCD();
// block the thread until a button is pressed
buttons.waitForAnyPress();
// get a port instance
Port portS1 = ev3brick.getPort("S1");
// Get an instance of the Ultrasonic EV3 sensor
EV3UltrasonicSensor sonicSensor = new EV3UltrasonicSensor(portS1);
// get an instance of this sensor in measurement mode
SampleProvider sonicdistance = sonicSensor.getDistanceMode();
// initialize an array of floats for fetching samples
float[] sample = new float[sonicdistance.sampleSize()];
// fetch a sample
sonicdistance.fetchSample(sample, 0);
while (buttons.getButtons() != Keys.ID_ESCAPE) {
lcddisplay.clear();
lcddisplay.drawString("distance: " + sample[0], 0, 1);
try {
//Thread.sleep(10000);
} catch (Exception e) {
}
}
}
}
在上面的代码中，要使用超声波传感器，您必须创建一个UltrasonicSensor的实例:
// get a port instance
Port portS1 = ev3brick.getPort("S1");
// Get an instance of the Ultrasonic EV3 sensor
EV3UltrasonicSensor sonicSensor =
new EV3UltrasonicSensor(portS1);
您必须指出您将传感器插入了哪个端口。在本例中，超声波插入端口 1。如果您的程序需要知道从传感器到任何对象的距离，那么您需要使用 getDistanceMode()方法:
// get an instance of this sensor in measurement mode
SampleProvider sonicdistance =
sonicSensor.getDistanceMode();
// initialize an array of floats for fetching samples
float[] sample = new float[sonicdistance.sampleSize()];
// fetch a sample
sonicdistance.fetchSample(sample, 0);
总之，超声波传感器可以帮助你的 EV3 机器人测量距离，观察物体的位置。如你所知，超声波传感器使用与蝙蝠相同的科学原理；也就是说，它通过计算声波检测到物体然后返回所需的时间来测量距离。通常，与柔软织物制成的物体或弯曲形状的物体相比，具有坚硬表面的较大物体返回的读数最好，因为后者很难被传感器检测到。
触摸传感器编程实践
到目前为止，你的机器人对他们的环境一无所知。现在你要开始用一些传感器来练习，这样机器人就可以判断如何最好地移动。在这一部分中，你的任务是给你的机器人增加触觉和超声波传感器，这样它就可以在周围环境中移动，根据需要执行避障动作。开始时，机器人应该沿直线前进，直到遇到障碍物。一旦检测到障碍物，你的机器人应该尝试一个回避策略。最简单的操作是后退，旋转远离物体，然后再次前进。你的机器人应该继续移动和躲避物体，直到程序被用户终止(例如，按下退出按钮)。你实际上应该产生两个不同的程序:一个只使用触摸传感器，另一个只使用超声波传感器。
在本练习中，您将开发一个程序，将触摸传感器放置在机器人的前面，并编写一个程序，使机器人能够绕过障碍物。伪代码可能如下，其中有一个循环不断询问传感器是否有问题以及是否应该采取规避措施。在本练习中，我们使用的 leJOS 版本是 leJOS NXJ。
while true
Move forward
if (touch sensor is pressed)
move backward
appropriate delay
turn right
下面的示例程序ch10p3.java说明了如何实现这个目标。
//******************************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch10p3.java
//This program has the robot move forward until the touch sensor is //activated.
//When the sensor is activated the robot will back up, rotate 90 degrees and continue moving.
//******************************************************************************************
import lejos.nxt.Button;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.robotics.navigation.DifferentialPilot;
public class ch10p3 {
public static void main(String[] args) {
// set up differential pilot and nav path controller to // use for
// navigation
DifferentialPilot pilot = new DifferentialPilot(4.32f, 12.2f, Motor.A,
Motor.C);
pilot.setRotateSpeed(60); // rotate speed set slower to // prevent slipping
// set up touch sensor
TouchSensor touch = new TouchSensor(SensorPort.S1);
// wait to begin
Button.waitForPress();
// move forward until touch sensor is pressed
while (!Button.ESCAPE.isPressed()) {
pilot.forward();
// if sensor is pressed, stop, rotate 90 degrees // then continue
if (touch.isPressed()) {
pilot.stop();
pilot.travel(-10);
pilot.rotate(90);
}
}
}
}
超声波传感器编程实践
在本练习中，您将编写一个程序，让您的机器人在一个区域内漫游，使用超声波传感器避免与墙壁碰撞。它应该在您按下 ENTER 键时开始，在您按下 ESCAPE 键时停止，不断显示其 LCD 上显示的距离。此外，如果测距仪检测到障碍物，该程序的代码应该能够指示机器人后退一定距离，旋转远离物体，然后再次前进。下面是一个示例程序ch10p4.java，向您展示如何实现这个目标。
//**********************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3 ch10p4.java
//This program will move the robot forward until the ultrasonic //sensor detects an
//object within 25cm. When the object is detected the robot will //back up to 30cm,
//rotate 90 degrees, then continue moving.
//**********************************************************************************
import lejos.nxt.Button;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.DifferentialPilot;
public class ch10p4 {
public static void main(String[] args) {
int distance;
// set up differential pilot and nav path controller to // use for
// navigation
DifferentialPilot pilot = new DifferentialPilot(4.32f, 12.2f, Motor.A,
Motor.C);
// rotate speed set slower to prevent slipping
pilot.setRotateSpeed(180);
// set up ultrasonic sensor
UltrasonicSensor ultraSonic = new UltrasonicSensor(SensorPort.S1);
// wait to begin
Button.waitForPress();
// move forward until distance from object is 25 cm
while (!Button.ESCAPE.isPressed()) {
distance = ultraSonic.getDistance();
while (distance > 25) {
pilot.forward();
distance = ultraSonic.getDistance();
}
// if object is closer than 25 cm backup to 30 cm
if (distance <= 25) {
while (distance <= 30) {
pilot.backward();
distance = ultraSonic.getDistance();
}
// rotate 90 degrees and continue loop
pilot.rotate(90);
}
}
}
}
摘要
在本章中，您了解了触摸和超声波传感器的功能及其工作原理。您还熟悉了超声波传感器和触摸传感器的用法，并学习了如何应用 leJOS NXJ Java 编程语言来控制和操作触摸传感器和超声波传感器，以使机器人具有与其环境交互的能力。
在下一章中，您将使用您创建的轮式机器人，并添加光传感器和颜色传感器，以使您的机器人在房间中移动时更加了解周围的环境。
十一、光传感器和颜色传感器简介
乐高 MindStorm EV3/NXT 套件带有四个常见的传感器:触摸传感器，颜色传感器，光传感器和超声波传感器。继第十章中的触摸传感器和超声波传感器之后，在本章中，您将为轮式机器人添加光传感器和颜色传感器，以提高它在房间内移动时对周围环境的感知。
特别是，本章将涵盖以下主题:
- 光传感器简介
- 颜色传感器简介
- 颜色和光线传感器的编程实践
光敏感元件
在 NXT 2.0 中，光传感器和颜色传感器集成到同一个硬件中。如图 11-1 所示，光线传感器由传感器正面的一个小透镜组成。
图 11-1。
Lego NXT 2.0 light sensor
光传感器测量由镜头捕获的光的强度，然后执行一组功能。通过将光传感器指向下方，机器人可以跟随一条黑线或由任何其他颜色组成的线。通过使用传感器，您还可以防止您的机器人移动到线的边缘，因为当物体远离时，光值会显著降低。换句话说，远处的物体反射的光不如近处的物体多。此外，光传感器可以区分暗物体和亮物体，因为暗物体反射回的光较少。
要使用光线传感器，首先使用以下构造函数创建它的一个实例:
public ColorSensor(SensorPort port);
然后，一旦有了ColorSensor的实例，使用方法setFloodlight关闭颜色检测，这样ColorSensor通过使用Color.NONE只检测环境光，如下所示。然后使用getLightValue()方法返回一个 0 到 100 之间的数字，其中灯光越亮，灯光值越高。
setFloodlight(Color.NONE)
int lightvalue = getLightValue()
下面是一个示例程序，它使用连接到 S3 港的光线传感器读取当前光线值。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0 ch11p1.java
//an example for light sensor testing
//******************************************************************
import lejos.nxt.*;
import lejos.robotics.Color;
public class ch11p1 {
public static void main(String[] args) throws Exception {
// Get an instance of the color/light sensor of NXT 2.0
ColorSensor lightsensor = new ColorSensor(SensorPort.S1);
// turn off the color detection, detecting light only
lightsensor.setFloodlight(Color.NONE);
LCD.clear();
// keep receiving light value until an Escape button is // pressed
while (!Button.ESCAPE.isPressed()) {
LCD.drawInt(lightsensor.getLightValue(), 4, 0, 0);
}
// clean out the LCD screen
LCD.clear();
}
}
颜色传感器
如前所述，在 Lego NXT 2.0 中，颜色传感器和光线传感器都集成在同一个硬件中。与光传感器类似，颜色传感器也可以用于检测不同的颜色。如图 11-1 所示，在硬件传感器的侧面有一个内置的从多色发光二极管发出红色、蓝色或绿色光的灯。通过实现ColorDetector接口，颜色传感器可以读取红色、绿色和蓝色值，或者它可以从调色板中识别颜色。要识别简单的预定义颜色，可以使用getColorID()方法，该方法返回一个表示颜色常数的整数。
int getColor()
这些值在lejos.robotics.Color类中预定义。例如，Color.GREEN表示绿色光谱中的颜色值。您也可以使用以下代码检索 RGB 值:
ColorPick cp = new ColorSensor(SensorPort.S1);
Color colorvalue = cp.getColor();
int green = colorvalue.getGreen();
Color 类可以通过使用Color.getColor()方法进一步识别色谱，该方法从调色板中返回一个颜色常数。
下面是一个示例程序，它通过识别光线中的不同颜色来测试您的颜色传感器是否正常工作。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0 ch11p2.java
//an example for color sensor testing
//******************************************************************
import java.util.ArrayList;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.robotics.Color;
import lejos.util.Stopwatch;
import sensors.ColorStruct;
import sensors.SensorControl;
import sensors.UnsupportedSensorException;
/**
* Demonstrates the use of color sensors
*/
public class ch11p2 {
// The port to use for color sensing
private static SensorPort _colorPort = SensorPort.S1;
// Stores the number of characters that fit on one line of the // LCD screen.
private static int LCD_DISP_WIDTH = 16;
public static void main(String[] args) {
try {
runColorSenseDemo();
} catch (UnsupportedOperationException e) {
handleError(e, true);
} catch (Throwable e) {
handleError(e, false);
}
}
/**
* Demonstrate color sensing
*
* @throws UnsupportedSensorException
* Thrown if _colorPort does not specify a valid
* color sending
* port
*/
private static void runColorSenseDemo() throws UnsupportedSensorException {
LCD.drawString("Press to begin…", 0, 0);
Button.waitForPress();
SensorControl sControl = new SensorControl(null, null, _colorPort, null);
while (!Button.ESCAPE.isPressed()) {
ColorStruct cvalue = sControl.getSensedColor();
Color cv = sControl.getRGBColor();
LCD.clear();
LCD.drawString("Color: " + cvalue, 0, 0);
LCD.drawString("Sensed:", 0, 1);
LCD.drawString(
cv.getRed() + " " + cv.getGreen() + " " + cv.getBlue(), 0,2);
}
}
/**
* Prints an error on the LCD screen so that it does not run
* off the screen,
* then waits for escape to be pressed to continue.
*
* @param message
* The message to display on the LCD screen.
* @param expected
* Flag indicating if the exception was expected or
* caught as
* part of a blanket-catch.
*/
private static void handleError(Throwable ex, boolean expected) {
String message;
if (expected) {
message = "ERROR: " + ex.getClass().toString() + ex.getMessage();
} else {
message = "UNEXPECTED ERR: " + ex.getClass().toString() + ex.getMessage();
}
LCD.clear();
ArrayList<String> messageSplit = new ArrayList<String>();
// split the message in 16-character segments
while (message.length() > LCD_DISP_WIDTH) {
messageSplit.add(new String(message.substring(0, LCD_DISP_WIDTH)));
message = message.substring(LCD_DISP_WIDTH, message.length());
}
int printRow = 0;
Stopwatch sw = new Stopwatch();
// print all of the messages
for (String msg : messageSplit) {
LCD.drawString("Here", 0, 4);
LCD.drawString("CLS:" + ex.getClass().toString(), 0, 5);
sw.reset();
while (sw.elapsed() < 1000)
Thread.yield();
LCD.drawString(msg, 0, printRow);
printRow++;
}
LCD.refresh();
LCD.drawString("Press to exit…", 0, printRow);
Button.waitForPress();
}
}
颜色和光线传感器的编程实践
在本编程练习中，您的任务是设计并编程 Mindstorms NXT 2.0 机器人，使其在两个预定义的路线中导航。第一个球场是由白底黑线定义的区域，第二个球场配备了障碍物——也就是黑线上的两个盒子。你的机器人应该认识到什么时候有障碍物挡住了它的路，绕过障碍物，然后重新加入这条路，继续走到路的尽头。图 11-2 展示了你的机器人将要探索的两条路线，其中红色方框表示障碍物。
图 11-2。
Two courses for programming practice with color/light sensor
在本编程练习中，您将创建一个具有以下功能的循线机器人:
- 你的机器人可以直接沿着黑线从起点到终点。您不必硬编码颜色来指示您是跟随白色背景上的黑色轨迹还是黑色背景上的白色轨迹。相反，您的程序允许您在启动时选择线条和背景颜色，然后等待按钮启动运行。
- 您的机器人可以避开到达终点途中的障碍，然后它可以顺利地重新加入原路线，直到到达目的地。
下面的程序，ch11p3. java被设计成没有任何障碍地跟随第一个程序。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0 ch11p3.java
//This program allows your robot to follow a line.
//If line is lost the robot will rotate left and right at increasing //angles
//until the line is found again.
//******************************************************************
import lejos.nxt.Button;
import lejos.nxt.ColorSensor;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.robotics.navigation.DifferentialPilot;
public class ch11p3
{
public static void main(String[] args)
{
// set up differential pilot and nav path controller to // use for navigation
DifferentialPilot pilot = new DifferentialPilot(4.32f, 12.2f, Motor.A, Motor.C);
pilot.setTravelSpeed(4);
//set up color sensor
ColorSensor colorSense = new ColorSensor(SensorPort.S1);
colorSense.setFloodlight(false);
//used to store values returned by color sensor
//follow is color robot is to follow, search is value //returned by sensor when searching
int follow, search;
//degrees robot will rotate when searching for line
int rotation;
//calibrate sensor
LCD.drawString("Place color sensor\nabove color to follow", 0, 0);
Button.waitForPress();
follow = colorSense.getColorID();
// place robot on start and wait for button press to
// begin main loop
LCD.clear();
LCD.drawString("Place robot", 0, 0);
Button.waitForPress();
// main loop
// follow line. if line is lost turn left and right to // search for it
while(!Button.ESCAPE.isPressed())
{
rotation = 5;
search = colorSense.getColorID(); //make sure we // are still on the line
//line is found continue forward
while(search == follow)
{
pilot.forward();
search = colorSense.getColorID();
}
//line lost
while(search != follow)
{
pilot.rotate(rotation); //rotate right
search = colorSense.getColorID();
if(search == follow)
break; //found line again exit loop
else
{
pilot.rotate(-rotation * 2); //rotate left back to start then to left position
search = colorSense.getColorID();
if(search == follow)
break;
//found line again exit loop
pilot.rotate(rotation);
//rotate back to center
}
rotation+=5; //increase angle of rotation // and continue search
}//end search
}//end main loop
}//end main()
}//end ch11p3
下面的程序ch11p4.java，是设计来跟随第二条路线，避开任何障碍物的。
//******************************************************************
// Wei Lu Java Robotics Programming with Lego EV3/NXT2.0 ch11p4.java
// Program that allows your robot to follow a line. It uses the
// ultrasonic
// sensor to detect if an object is in its path while following the // line.
// If an object is detected the robot will leave the line and travel // around
// the object. It will then search for the line and continue
// traveling.
//******************************************************************
import lejos.nxt.Button;
import lejos.nxt.ColorSensor;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.DifferentialPilot;
public class ch11p4 {
public static void main(String[] args) {
// set up differential pilot and nav path controller to // use for
// navigation
DifferentialPilot pilot = new DifferentialPilot(4.32f, 12.2f, Motor.A,
Motor.C);
pilot.setTravelSpeed(5);
UltrasonicSensor ultra = new UltrasonicSensor(SensorPort.S4);
// set up color sensor
ColorSensor colorSense = new ColorSensor(SensorPort.S1);
colorSense.setFloodlight(false);
// used to store values returned by color sensor
// follow is color robot is to follow, search is value // returned by
// sensor when searching
int follow, search;
// degrees robot will rotate when searching for line
int rotation;
// calibrate sensor
LCD.drawString("Place color sensor\nabove color to follow", 0, 0);
Button.waitForPress();
follow = colorSense.getColorID();
// place robot on start and wait for button press to
// begin main loop
LCD.clear();
LCD.drawString("Place robot", 0, 0);
Button.waitForPress();
// main loop
// follow line. if line is lost turn left and right to // search for it
while (!Button.ESCAPE.isPressed()) {
rotation = 5;
search = colorSense.getColorID();
// make sure we // are still on the// line
// line is found continue forward
while (search == follow) {
// object detected
if (ultra.getDistance() <= 10) {
// execute 90 degree turn to
// navigate around object
pilot.rotate(90);
pilot.travel(10);
pilot.rotate(-90);
pilot.travel(30);
pilot.rotate(-90);
search = colorSense.getColorID();
// find line again
while (search != follow) {
pilot.forward();
search = colorSense.getColorID();
// line found, rotate until // following again
if (search == follow) {
pilot.rotate(90);
break;
}
}
}
// continue following line
pilot.forward();
search = colorSense.getColorID();
}
// line lost
while (search != follow) {
pilot.rotate(rotation); // rotate right
search = colorSense.getColorID();
if (search == follow)
break; // found line again exit loop
else {
pilot.rotate(-rotation * 2);
// rotate left back to start
// then to left position
search = colorSense.getColorID();
if (search == follow)
break;
// found line again exit loop
pilot.rotate(rotation);
// rotate back to center
}
rotation += 5; // increase angle of
// rotation and continue search
}// end search
}// end main loop
}// end main()
}
摘要
在本章中，您学习了光线传感器和颜色传感器的功能，以及这两种传感器的工作原理。您还熟悉了光传感器和颜色传感器的使用，并学习了如何应用 leJOS NXJ JAVA 编程来控制和操作它们，以使您的机器人能够与其周围环境进行交互。
在下一章，你将开始学习基于 leJOS 中定义的包容架构的行为编程。您还将了解如何在 leJOS Java 编程中应用仲裁器来控制和操作触摸传感器、超声波传感器和颜色传感器，按照一组行为的顺序，为机器人提供与其周围环境交互的能力。
十二、行为编程导论
机器人有能力通过使用行为编程来实现一系列类似人类的行为，如感知、规划和行动。在这一章中，你将学习基于 leJOS 中定义的包容架构的行为编程。您还将了解如何在 leJOS JAVA 编程中应用仲裁器，以一组行为的特定顺序来控制和操作触摸传感器、超声波传感器和颜色传感器，从而赋予机器人与其周围环境交互的能力。
特别是，本章将涵盖以下主题:
- 行为编程导论
- 行为 API 函数
- 行为编程的设计模式
- 行为编程实践
行为编程导论
行为编程是基于感觉-计划-行动控制模型的，在你开始编码之前，它需要更多的计划。通过感知，机器人通过视觉、触觉、声音和距离获取周围环境的信息。通过规划，机器人利用感官信息来决定行动。通过行动，机器人使用它的运动部件来完成计划。通过实现 Sense-Plan-Act 模型，从理论上讲，熟悉行为控制模型的其他程序员会更容易理解您的代码。此外，通过行为编程，您可以更容易地从整体结构中添加或删除特定的行为，而不会对代码的其余部分产生负面影响。
基于行为的机器人编程是设定一系列要执行的行为或要完成的目标的行为。几个简单的行为可以组合起来，理论上可以在现实世界中形成一个非常复杂的逻辑模型。基于行为的机器人编程理论最初是受昆虫的启发，被称为“硬连线思维”，由麻省理工学院人工智能实验室的罗德尼·布鲁克斯于 1983 年提出，如图 12-1 所示。
图 12-1。
Insects “hard-wired thinking ”
包容架构是一种广泛用于机器人编程的方法，它与基于行为的机器人技术密切相关。包容架构是一种将复杂的智能行为分解成许多更简单的行为模块的方法。依次地，这些被组织成不同的层。每一层都实现代理的一个特定目标，更高的层越来越抽象。例如，机器人的底层可以是“躲避物体”相比之下，在那一层之上的可能是“四处游荡”的人下一个更高的层次可能是“探索世界”，等等。这些层中的每一层都可以访问所有的传感器数据，并为致动器生成一组动作。使用这种方法，较低层可以充当快速适应机制，而较高层则努力实现总体目标。
Java leJOS 中实现的行为编程的概念遵循以下四条规则:
- 任何时候都应该只有一种行为是活动的，并且在机器人的控制之下。
- 每个行为都有一个预定义的固定优先级数。
- 每种行为都有能力决定是否应该控制。
- 优先级高于任何其他行为的活动行为应该进行控制。
一般来说，机器人的行为编程是一个解决问题的过程，在设计程序时，您可以按照所示的顺序执行以下步骤:
- 你想让机器人做什么？
- 机器人必须如何表现才能完成任务？
- 把复杂的任务分解成几个简单的行为，包括给每个行为分配一个优先级，一个优先级高的行为可能会停止当前的行为。
- 使用 Java 编程语言创建程序。
- 运行程序。
- 机器人的行为符合要求吗？(它正确执行任务了吗？)如果没有，请执行以下操作:
- 先检查机器人。如果有问题，你能解决吗？
- 接下来，检查程序。有问题吗？你能修好它吗？
- 最后回到开头，重读任务。你的程序真的告诉机器人它应该做什么吗？
行为 API 函数
行为 API 有一个接口和一个类，即行为接口和仲裁器类。行为接口定义了单独的行为类，包括三个公共方法:takeControl()、action()和suppress()。一旦创建了所有的行为，就使用一个仲裁器来管理所有这些行为，以确定应该在什么时候激活哪一个行为。仲裁器类和行为接口位于lejos.subsumption包中。图 12-2 展示了 Java leJOS 中行为编程的工作原理。如图所示，行为接口中的三种方法都非常简单。假设您的机器人有三个独立的行为，那么您需要创建三个单独的类，每个类实现行为接口。一旦这些类完成，您的代码应该将行为对象交给仲裁器。仲裁器在lejos.subsumption.Arbitrator包中，其构造函数如下:
图 12-2。
Behavior programming
public Arbitrator(Behavior[] behaviors, boolean returnWhenInactive)
这将创建一个仲裁器对象，它控制每个行为何时激活，其中参数behaviors用于索引数组中每个行为的优先级(例如，behaviors[0]具有最低优先级)，如果参数boolean returnWhenInactive的值为真，当没有行为想要控制时，程序退出。否则，程序将一直运行，直到按下 NXT 模块上的 Enter 和 Escape 按钮而关闭。只需使用一个公共方法start()，就可以启动仲裁系统。
当仲裁器对象被实例化时，它被赋予一个行为对象数组。一旦有了这些，就调用start()方法，并开始仲裁和确定哪个行为将被激活。仲裁器对每个行为对象调用takeControl()方法，从行为数组中索引号最高的对象开始。它按照优先级递减的顺序工作，直到找到想要控制的行为。如果此行为的优先级指数大于当前活动行为的优先级指数，则活动行为将被抑制。在这之后，对这个索引的行为调用action()方法。因此，如果有几个行为希望取得控制权，那么只有优先级最高的行为才会被激活。
行为编程的设计模式
为了控制行为控制系统的性能，当调用suppress()时，迅速终止action()方法是非常重要的。为了实现这一点，leJOS 实际上为行为编程定义了一个设计模式。在这个设计模式中，仲裁器包含一个监视线程来循环遍历每个行为，检查takeControl()方法来查看行为是否应该被激活。它从具有最高索引号的行为开始，沿着数组向下进行。一旦遇到一个应该控制的行为，监视器线程就对这个活动行为执行suppress(),然后再从头开始检查每个行为。
下面是一个真实世界的例子，以及如何使用设计模式概念来解决问题。
如图 12-3 所示，你想直线前进，当机器人前方有物体时，你可以绕过这个物体。在这种情况下，您有两种行为:
- 行为 1:当机器人前方检测到有物体时，向前移动。停止当前行为，开始行为 2。
- 行为 2:旋转机器人，绕过物体。
图 12-3。
Scenario for design pattern of behavior programming
图 12-4 展示了行为 1 向前发展的设计模式。
图 12-4。
Design pattern of Behavior 1 moving forward
图 12-5 显示了行为 2 旋转到旁路的设计模式。图 12-6 接着说明了如何使用设计模式方法管理这两种行为，包括如何设置一组行为以及如何启动编程的行为。
图 12-6。
Managing behaviors using design pattern
图 12-5。
Design pattern of Behavior 2 rotating to bypass
总而言之，使用设计模式来管理行为的想法在下面的例子中得到了说明:
- 当仲裁器对象被实例化时，它被赋予一个数组中的一组行为。
- 然后调用
start()方法，仲裁器开始确定哪些行为应该被激活。 - 仲裁器对每个行为调用
takeControl()方法，从数组中索引号最高的对象开始。 - 然后仲裁器遍历每个行为，直到找到想要控制的行为。当它遇到一个时，它执行该行为的
action()方法一次，并且只执行一次。 - 如果两个行为都想控制，那么只有更高级的行为才被允许。
行为编程的编程实践
在本编程练习中，您将创建一个循线机器人，该机器人通过基于包容架构的行为编程满足以下要求:
- 行为 1:你的机器人可以从起点一直沿着绿线走，直到到达第一个障碍箱。
- 行为 2:你的机器人随后可以应用触摸传感器来检测这个障碍物，然后绕过第一个障碍物；也就是你可以让你的机器人向后移动，做一个适当的延迟，然后右转。
- 行为 3:在你的机器人绕过第一个障碍后，它将继续前进，然后回到这条线，它将沿着一条蓝色的线从它遇到的第一个障碍一直走到第二个障碍箱。
- 行为 4:你的机器人然后使用超声波传感器避免与第二个障碍物碰撞。一旦测距仪检测到第二个障碍物，机器人就会旋转远离物体，然后继续前进。
- 行为五:机器人在避开终点途中的障碍物后，再平滑地重新加入原来的蓝线，直到到达终点。
图 12-7 展示了这种编程实践的过程。
图 12-7。
Moving area for your programing practice
//******************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0 ch12p1.java
//This program implements the leJOS behavior capabilities.
//The robot will begin following a green line. When the touch
//sensor is activated it will move around an object and search for a
//blue line. Then when it travels within 15cm of an object it will //avoid it
//and return to the blue line again.
//******************************************************************************
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
public class ch12p1 {
public static void main(String[] args) {
// create a sensors object to send to the behavior
// classes
Sensors sensors = new Sensors();
// prompt to begin
LCD.clear();
LCD.drawString("Press to begin", 0, 0);
Button.waitForPress();
// set up behavior classes
Behavior b1 = new FollowGreen(sensors);
Behavior b2 = new TouchAvoid(sensors);
Behavior b3 = new FollowBlue(sensors);
Behavior b4 = new SonicAvoid(sensors);
// create behavior array
Behavior[] bArray = { b1, b3, b2, b4 };
// send array to arbitrator and begin
Arbitrator arby = new Arbitrator(bArray);
arby.start();
}
}
//*********************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0
// FollowGreen.java
// This behavior follows a green line
//*********************************************************
import lejos.nxt.Motor;
import lejos.robotics.Color;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;
public class FollowGreen implements Behavior {
private boolean suppressed;
// used to store values returned by color sensor
// follow is color robot is to follow, search is value //returned by sensor
// when searching
private int follow, search;
// degrees robot will rotate when searching for line
int rotation;
// set up differential pilot and nav path controller to use //for navigation
private DifferentialPilot pilot = new DifferentialPilot(4.32f, 12.2f,
Motor.A, Motor.C);
Sensors sensors;
public FollowGreen(Sensors globalSensors) {
sensors = globalSensors;
suppressed = false;
pilot.setTravelSpeed(4);
sensors.colorSense.setFloodlight(false);
follow = Color.GREEN;
}
@Override
public boolean takeControl() {
return true;
}
@Override
public void action() {
suppressed = false;
search = sensors.colorSense.getColorID();
// move forward while line is green
while (!suppressed) {
Thread.yield();
while (search == follow && !suppressed) {
pilot.forward();
search = sensors.colorSense.getColorID();
Thread.yield();
}
// line lost
while (search != follow && !suppressed) {
pilot.rotate(rotation); // rotate right
search = sensors.colorSense.getColorID();
Thread.yield();
if (search == follow)
break; // found line again exit loop
else {
pilot.rotate(-rotation * 2);
// rotate left back to start
// then to left position
search = sensors.colorSense.getColorID();
Thread.yield();
if (search == follow)
break; // found line again //exit loop
pilot.rotate(rotation); // rotate //back to center
}
rotation += 5; // increase angle of //rotation and continue search
Thread.yield();
}// end search
Thread.yield();
}
}
@Override
public void suppress() {
suppressed = true;
}
}// end FollowGreen
//*********************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0
// Sensors.java
//Class to store sensor objects to make them available in all //behavior classes
//*********************************************************************************
import lejos.nxt.ColorSensor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
public class Sensors {
ColorSensor colorSense;
TouchSensor touch;
UltrasonicSensor sonic;
boolean touchPressed = false; // used for take control method //in followBlue
//behavior
public Sensors() {
// set up color, touch, and sonic sensors
colorSense = new ColorSensor(SensorPort.S1);
touch = new TouchSensor(SensorPort.S2);
sonic = new UltrasonicSensor(SensorPort.S4);
}
}
//***************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0 //SonicAvoid.java
//This behavior avoids objects using the ultra sonic sensor
//***************************************************************************
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.robotics.Color;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;
public class SonicAvoid implements Behavior {
// set up differential pilot
private DifferentialPilot pilot = new DifferentialPilot(4.32f, 12.2f,
Motor.A, Motor.C);
private boolean suppressed;
private int follow, search;
Sensors sensors;
public SonicAvoid(Sensors globalSensors) {
pilot.setTravelSpeed(4);
suppressed = false;
follow = Color.WHITE;
sensors = globalSensors;
}
// take control when within 15 cm of an object and on a blue //line
@Override
public boolean takeControl() {
return (sensors.sonic.getDistance() < 15 && sensors.colorSense
.getColorID() == Color.WHITE);
}
@Override
public void action() {
LCD.drawString("Sonic triggered", 0, 0);
suppressed = false;
// stop and move around object
pilot.stop();
// execute 90 degree turn to navigate around object
pilot.rotate(90);
pilot.travel(10);
pilot.rotate(-90);
pilot.travel(30);
pilot.rotate(-90);
search = sensors.colorSense.getColorID();
// find line again
while (search != follow && !suppressed) {
LCD.drawString("HERE I AM", 0, 0);
pilot.forward();
search = sensors.colorSense.getColorID();
// line found, rotate until following again
if (search == follow) {
pilot.rotate(90);
suppressed = true;
break;
}
Thread.yield();
}
}
@Override
public void suppress() {
suppressed = true;
}
}
//**************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0 //TouchAvoid.java
//This class represents a robotic behavior when the touch sensor
//is activated
//**************************************************************************
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.robotics.Color;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;
public class TouchAvoid implements Behavior {
// set up differential pilot
private DifferentialPilot pilot = new DifferentialPilot(4.32f, 12.2f,
Motor.A, Motor.C);
private boolean suppressed;
private int follow, search;
Sensors sensors;
public TouchAvoid(Sensors globalSensors) {
pilot.setTravelSpeed(4);
suppressed = false;
follow = Color.WHITE; // color to follow when
// relocating line
sensors = globalSensors;
}
@Override
public boolean takeControl() {
return sensors.touch.isPressed(); // behavior takes
// control when touch
// sensor is pressed
}
@Override
public void action() // avoid the object
{
LCD.drawString("BUTTON IS PRESSED", 0, 0);
sensors.touchPressed = true; // set flag in sensor //class so robot knows
// to follow different color later
suppressed = false;
// moves to avoid object
pilot.stop();
pilot.travel(-15);
// execute 90 degree turn to navigate around object
pilot.rotate(90);
pilot.travel(10);
pilot.rotate(-90);
pilot.travel(30);
pilot.rotate(-90);
search = sensors.colorSense.getColorID();
// find line again
while (search != follow && !suppressed) {
// LCD.clear();
pilot.forward();
search = sensors.colorSense.getColorID();
LCD.drawString("search: " + Integer.toString(search), 0, 0);
LCD.drawString("follow: " + follow, 0, 1);
// line found, rotate until following again
if (search == follow) {
pilot.rotate(90);
suppressed = true;
break;
}
Thread.yield();
}
}
@Override
public void suppress() {
suppressed = true;
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0
//FollowBlue.java
//This behavior follows a blue line
//******************************************************************
import lejos.nxt.ColorSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.robotics.Color;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;
public class FollowBlue implements Behavior {
private boolean suppressed;
// used to store values returned by color sensor
// follow is color robot is to follow, search is value //returned by sensor
// when searching
private int follow, search;
// degrees robot will rotate when searching for line
int rotation;
// set up differential pilot and nav path controller to use //for navigation
private DifferentialPilot pilot = new DifferentialPilot(4.32f, 12.2f,
Motor.A, Motor.C);
Sensors sensors;
public FollowBlue(Sensors globalSensors) {
sensors = globalSensors;
suppressed = false;
pilot.setTravelSpeed(4);
sensors.colorSense.setFloodlight(false);
follow = Color.WHITE;
}
@Override
public boolean takeControl() {
return sensors.touchPressed; // we want this to be the default behavior
// after the touch sensor has been
// pressed
}
@Override
public void action() {
suppressed = false;
search = sensors.colorSense.getColorID();
// move forward if the line is the right color
while (!suppressed) {
Thread.yield();
while (search == follow && !suppressed) {
pilot.forward();
search = sensors.colorSense.getColorID();
Thread.yield();
}
// line lost
while (search != follow && !suppressed) {
pilot.rotate(rotation); // rotate right
search = sensors.colorSense.getColorID();
Thread.yield();
if (search == follow)
break; // found line again exit loop
else {
pilot.rotate(-rotation * 2);
// rotate left back to start
// then to left position
search = sensors.colorSense.getColorID();
Thread.yield();
if (search == follow)
break;
// found line again exit loop
pilot.rotate(rotation);
// rotate back to center
}
rotation += 5; // increase angle of //rotation and continue search
Thread.yield();
}// end search
Thread.yield();
}
}
@Override
public void suppress() {
suppressed = true;
}
}// end FollowBlue
摘要
在这一章中，你学习了如何使用 leJOS 中定义的包容架构进行行为编程。您还学习了如何在 leJOS Java 编程语言中应用仲裁器，以一组行为的特定顺序控制和操作触摸传感器、超声波传感器和颜色传感器，从而赋予机器人与其周围环境交互的能力。
在下一章中，您将学习 Java leJOS 多线程编程的基本概念，然后学习如何应用它来控制和操作颜色传感器、触摸传感器和超声波传感器，以使您的机器人能够与其周围环境进行交互。
十三、使用 Java leJOS 的多线程编程
多线程是一个非常著名的编程特性，它允许您同时执行多个作业。在为机器人开发程序时，您需要将这种编程特性视为您的编程架构的基础。
在本章中，您将学习 Java leJOS 多线程编程的基本概念，然后学习如何应用它来控制和操作颜色传感器、触摸传感器和超声波传感器，以使您的机器人具有与其周围环境交互的能力。
特别是，本章将涵盖以下主题:
- 线程概念介绍
- 如何在 leJOS 中使用类 thread
- Java leJOS 多线程编程实践
线程概念
通常，当您开始使用 Java 和 leJOS 进行开发时，您创建的程序会一个接一个地按顺序执行一组操作。例如，参见下面的程序结构，其中程序首先完成任务 1，然后完成任务 2、任务 3，依此类推，直到最后一个程序任务，即任务 10:
Program:
Task 1
Task 2
Task 3
...
Task 10
但是，如果您使用 Java 多线程，那么您可以并行执行机器人的任务，如下所示，其中程序同时执行所有 10 个任务:
Task 1 Task 2 Task 3 Task 4 Task 5 Task 6 Task 7 Task 8 Task 9 Task 10
这和我们身体并行执行的操作非常相似，比如呼吸、血液循环、消化、思考、行走。此外，人类大脑并行处理来自你身体传感器的信息，包括视觉、触觉、嗅觉、味觉和听觉。
许多当前的编程语言允许程序员指定并发执行的活动。这是通过使用线程来实现的。Java 中的线程是并行处理的最小单位。因此，在使用线程设计机器人程序时，您应该考虑以下想法:
- 管理无线通信的线程
- 管理移动子系统的线程
- 管理机器人装备的线程
- 一根管理机器人感官的线
机器人的设计过程是迭代的，因为这是一项复杂的任务。这不同于传统的一次执行一个动作的设计过程，只有在前一个动作完成后才进行下一个动作。这就是线程技术非常有用的地方。当你想在将来加入一个新的特性时，你只需要添加一个新的任务，一个线程，或者选择一个旧的线程并用新的功能更新它。
在 leJOS 中使用线程
在下面的例子ch13p1.java中，你将看到如何开发你的第一个线程，它由一个主程序管理。该程序可以在 NXT 2.0 LCD 上打印消息“Hello World”和变量i的值。(i计算屏幕上打印了多少条信息。)这个程序执行线程HelloWorldThread，直到你按下 NXT 2.0 砖块上的 ESCAPE 按钮。
//****************************************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0 ch13p1.java
//This program executes the thread HelloWorldThread to print the //message
//"Hello World" and the value of a counter to count how
//many messages printed on the screen.
//****************************************************************************
import lejos.nxt.Button;
import lejos.nxt.LCD;
public class ch13p1 {
private static HelloWorldThread hwt;
public static void main(String[] args) {
int i = 0;
hwt = new HelloWorldThread();
hwt.start();
try {
while (!Button.ESCAPE.isPressed()) {
LCD.drawString("Hello World: " + i, 0, 0);
++i;
}
} catch (Exception ex) {
} finally {
System.exit(0);
}
}
}
import lejos.nxt.*;
public class HelloWorldThread extends Thread {
private int i = 0;
public HelloWorldThread() {
}
public void run() {
while (true) {
LCD.drawString("Hello World:", 0, 0);
LCD.drawInt(i, 0, 1);
LCD.refresh();
i++;
}
}
}
要理解多线程代码，了解线程的生命周期非常重要。线程的可能状态包括以下几种:
- 开始
- 睡眠
- 中断的
- 产量
- 加入
- 中断
通常，Java leJOS 中的 thread 类有三种方法:Start()、IsAlive()和Sleep()。我现在将分别介绍每种方法。
方法开始()
当您定义一个线程来执行机器人的任务时，您只需使用如下代码启动它:
private static HelloWorldThread hwt;
hwt = new HelloWorldThread();
hwt.start();
方法 isAlive()
在许多情况下，当处理多个线程时，有必要通过使用以下代码来知道一个线程是否仍然活动:
public void run() {
while (true) {
LCD.drawString("Hello World:", 0, 0);
LCD.drawInt(i, 0, 1);
LCD.drawString("" + this.isAlive(), 0, 2); LCD.refresh();
}
}
方法睡眠()
在许多其他情况下，通过使用以下代码中的 sleep 方法生成一个定时暂停来确保其他任务已经完成是非常有用的:
import lejos.nxt.*;
public class SleepDemo {
private static String messages[] = {
"Java leJOS NXJ",
"Java leJOS PC API",
"Java leJOS Mobile API",
};
public static void main(String[] args) throws InterruptedException {
for(int i=0;i< messages.length; i++) {
Thread.sleep(1000);
System.out.println(Messages[i]);
}
}
}
用 Java leJOS 练习多线程
在这个示例程序ch13p2.java中，您将完成一个名为 Music 的多线程程序，这样您的程序就可以在播放音乐的同时在 LCD 屏幕上打印一个序列号。
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0 ch13p2.java
//This program demonstrates a threaded song/counter program
//******************************************************************
import lejos.nxt.Button;
public class ch13p2 {
private static Music mThread;
private static Counter cThread;
public static void main(String[] args) {
mThread = new Music();
cThread = new Counter();
mThread.start();
cThread.start();
try {
while (!Button.ESCAPE.isPressed()) {
Thread.yield();
}
System.exit(0);
} catch (Exception e) {
}
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0 Music.java
//Play a "song" consisting of predefined beeps on a thread
//******************************************************************
import lejos.nxt.Sound;
class Music extends Thread {
public void run() {
short Low_G = 392, B_Flat = 470, D = 588, C = 523, E_Flat = 627, F = 697, G = 784, A_Flat = 830;
short[] note = {a C, 600, G, 100, F, 100, G, 400, C, 400, 0, 600,
A_Flat, 100, G, 100, A_Flat, 200, G, 200, F, 400, B_Flat, 600,
F, 100, E_Flat, 100, F, 400, B_Flat, 400, Low_G, 600, G, 100,
F, 100, G, 200, F, 200, E_Flat, 200, D, 200, E_Flat, 600, D,
100, E_Flat, 100, F, 600, E_Flat, 100, F, 100, G, 200, F, 200,
E_Flat, 200, D, 200, C, 400, A_Flat, 400, G, 1400, A_Flat, 100,
G, 100, F, 100, G, 1400
};
for (int i = 0; i < note.length; i += 2) {
short w = note[i + 1];
Sound.playTone(note[i], w);
Sound.pause(w);
}
}
}
//******************************************************************
//Wei Lu Java Robotics Programming with Lego EV3/NXT2.0 Counter.java
//Continuously write an incrementing integer to the LCD
//******************************************************************
import lejos.nxt.LCD;
class Counter extends Thread {
public void run() {
for (int i = 0; i < 1000; ++i) {
LCD.drawInt(i, 0, 2);
LCD.refresh();
try {
Thread.sleep(1000);
} catch (Exception e) {
}
}
}
}
在第十一章中，你创造了一个循线机器人，它可以沿着绿线从一条线的起点直线前进到终点。您创建的机器人还可以在到达终点的途中避开障碍物，并可以顺利地重新加入原始线，直到到达终点。
在多线程编程的第二个练习中，您将创建一个与您在第十一章中创建的机器人非常相似的机器人。在这次练习中，机器人的主要目标是沿着一条绿线前进，如图 13-1 所示，如果发现障碍物，它就会停下来。您必须使用 leJOS 多线程编程来实现这个循线机器人。
图 13-1。
Programming the line-following robot with multithreading
在分析所涉及的任务时，你会发现这个程序有两个主要过程:(1)沿着绿线走，(2)发现途中的障碍。基于这种分析，您可以用线程设计和开发一个可伸缩的解决方案:实现一个用于跟随绿线的单独线程，实现另一个用于检测障碍物的单独线程，再实现一个用于在线程间交换数据的类。
import lejos.nxt.*;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.NavPathController;
public class ch13p3 {
private static data DE;
private static line LFObj;
private static object ODObj;
static double diam = 2.8;
static double trackwidth = 15.5;
static DifferentialPilot pilot = new DifferentialPilot(diam / 2.54,
trackwidth / 2.54, Motor.A, Motor.C);
static UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S4);
NavPathController nav = new NavPathController(pilot);// Attaches NavPath
public static void main(String[] args) {
DE = new data();
ODObj = new object(DE, pilot);
LFObj = new line(DE, pilot);
ODObj.start();
LFObj.start();
while (!Button.ESCAPE.isPressed()) {
}
LCD.drawString("Finished", 0, 7);
LCD.refresh();
}
}
public class data {
// ObstacleDetector
private boolean obstacleDetected = false;
// Robot has the following commands: Follow Line, Stop
private int CMD = 1;
public data() {
}
public boolean isObstacleDetected() {
return obstacleDetected;
}
public void setObstacleDetected(boolean obstacleDetected) {
this.obstacleDetected = obstacleDetected;
}
public int getCMD() {
return CMD;
}
public void setCMD(int cMD) {
CMD = cMD;
}
}
import lejos.nxt.*;
import lejos.robotics.Color;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.util.Stopwatch;
public class line extends Thread {
data DEObj;
private ColorSensor cs;
private UltrasonicSensor us;
private DifferentialPilot pilot;
static boolean leftfirst = true;
public line(data DE, DifferentialPilot pi) {
DEObj = DE;
cs = new ColorSensor(SensorPort.S3);
us = new UltrasonicSensor(SensorPort.S4);
pilot = pi;
}
public void run() {
if (DEObj.getCMD() == 1) {
if (cs.getColorID() == Color.GREEN) {
pilot.forward();
} else {
pilot.travel(1.5);
int found = 0;
LCD.clear(6);
double degrees = 10;
double look = 0, i = 1;
if (leftfirst != true) {
i = 1 * i;
}
LCD.drawString("Looking", 0, 6);
Stopwatch clock = new Stopwatch();
clock.reset();
while (clock.elapsed() < 30000) {
look = degrees * i;
pilot.rotate(look);
pilot.stop();
int batman = cs.getColor().getColor();
if (batman == Color.GREEN) {
LCD.clear(6);
LCD.drawString("Found it!", 0, 6);
found = 1;
break;
} else if (batman == Color.BLACK) {
found = 2;
break;
} else {
look = look * 2 * 1;
pilot.rotate(look);
batman = cs.getColor().getColor();
if (batman == Color.GREEN) {
pilot.stop();
LCD.clear(6);
LCD.drawString("Found it!", 0, 6);
found = 1;
break;
} else if (batman == Color.BLACK) {
found = 2;
break;
}
pilot.rotate(look * 1);
}
i++;
}
if (look < 0) {
leftfirst = false;
} else {
leftfirst = true;
}
}
} else {
pilot.stop();
}
}
}
import lejos.nxt.*;
import lejos.nxt.ColorSensor.Color;
import lejos.robotics.navigation.DifferentialPilot;
public class object extends Thread {
private data DEObj;
private UltrasonicSensor us;
private ColorSensor cs;
private final int securityDistance = 15;
private DifferentialPilot pilot;
public object(data DE, DifferentialPilot pi) {
DEObj = DE;
us = new UltrasonicSensor(SensorPort.S4);
cs = new ColorSensor(SensorPort.S3);
pilot = pi;
}
public void run() {
while (true) {
if (us.getDistance() > securityDistance) {
DEObj.setCMD(1);
} else {
DEObj.setCMD(0);
pilot.rotate(80);// left
Motor.B.rotateTo(90);
while (us.getDistance() < 15) {
pilot.forward();
}
pilot.travel(7);// offline
pilot.rotate(85);// back on track
pilot.travel(2);//
while (us.getDistance() < 30) {
pilot.forward();
}
pilot.travel(8);
pilot.rotate(85);
pilot.setTravelSpeed(1);
while (cs.getColor().getColor() != Color.GREEN) {
pilot.forward();
}
pilot.stop();
pilot.setTravelSpeed(2.25);
pilot.travel(2);
pilot.rotate(85);// left
Motor.B.rotateTo(0);
DEObj.setCMD(1);
}
}
}
}
摘要
在本章中，您学习了 Java leJOS 多线程编程的基本概念，以及如何应用这些概念来控制和操作颜色传感器、触摸传感器和超声波传感器，从而使您的机器人能够与其周围环境进行交互。
读完这本书并完成所有的编程练习后，让你的机器人编程更上一层楼。例如，尝试通过开发 Android 应用来管理和控制乐高 EV3 设备，并实现 Java 应用在各种乐高 EV3 设备之间建立无线连接，从而远程控制您的机器人。
