In Lab 2 we looked at using light sensors to follow a line. In this lab we will be looking at using the Navigation class to move the robot around on a 2-dimensional plane.

When programming Lego robots a 2-dimensional Cartesian coordinate system is used to describe locations. The coordinate system keeps track of two numbers, an x-Position and a y-Position. Suppose our robot wants to get from a starting point in the Cartesian space to an end point. Programming the robot to travel from one location to a goal location is usually more complicated than just moving in a straight line as there are usually obstacles that lie in the way. We can use a number of strategies to find a path ranging from choosing a random direction when we encounter an obstacle through to using search algorithms such as A*. In this session we are going to look at depth first search (DFS) for path planning.

The Navigator class tells the Pilot object how to drive to a specific location based on the Cartesian coordinate system.

Extract and open the RobotLabNav Project in Atom. Open the file RobotLabNav.java. To compile and run your code you can press F9 (make sure your robot is connected via the USB 2.0 port and is turned on).

The code we have given you contains the following lines:

import lejos.nxt.*;
import lejos.robotics.navigation.*;

public class RobotLabNav {

  public static void main(String[] args) {

      DifferentialPilot pilot;
      Navigator navbot;
      pilot = new DifferentialPilot(2.25f, 5.5f, Motor.A, Motor.C);
      navbot = new Navigator(pilot);

      LCD.clear();
      LCD.drawInt(50, 0, 0);
      LCD.drawInt(50, 5, 0);

      navbot.goTo(50, 50);
      navbot.waitForStop();  

  }//end class main
}//end RobotLabNav

The code instantiates a Navigator, prints the target coordinate (x = 50, y = 50) to the LCD screen, then drives the robot to that coordinate. Note that the units for the coordinate system will be the same as the units used for wheel diameter and track width in your constructor. The goTo() method is non-blocking and so needs a waitForStop() to complete the action.

In a Navigation class, the target coordinates are called waypoints. You can create your own method to generate a set of waypoints and then feed the coordinates to the Navigator using waypoint objects.

The Navigator makes use of a PoseProvider that keeps track of the robot positions in a route. The route is an instance of the path class, and it behaves as a first-in, first-out (FIFO) queue. When a waypoint is reached, it is removed from the route queue and the robot goes to the next waypoint.

• Waypoint wp = new Waypoint (50, 50); - creates a new waypoint object
• void goTo(wp); - Starts the moving toward the destination Waypoint. This method is non-blocking.
• void addWaypoint(new Waypoint(200, 200)); - Constructs an new waypoint and adds it to the end of the path.
• boolean waitForStop(); - Waits for the robot to stop for any reason. Returns true if the robot stopped at the final Waypoint of the path.

---

Task 3.1 Write a program so that the robot will start at A(0,0) go to B(50, 50), then go back to A(0,0), then go to C(-50, 50) then finally come back to A(0,0). Your robot should display destination coordinates to the LCD screen.

---

Depth-First Search (DFS)

Suppose we have 7 destinations S (0, 0), A (30, 0), B (30,40), C (30, 80), D(60, 0), E(60, 80), and G(90, 40). We know that roads exist between S and A, S and B, A and B, A and D, B and C, C and E, and D and G. We want to find a path between S (start) and G (goal).

We can represent this as a tree structure and do a DFS to find a path.

Extract and open the RobotLabDFS Project in Atom. Open the file RobotLabDFS.java.

This code is split into three parts:

1. Setting up the tree structure
2. Doing DFS
3. Getting the robot to follow the path

Setting up the tree structure

The code we have given you contains the following lines:

// create new nodes
Node S = new Node("S", 0, 0);
S.addChild(A);

We have set up a Node class (see Node.java) which stores the following information about the node:

• String name; - name of node
• Node pred; - predecessor
• Boolean explored = false; - whether node has been explored
• ArrayList<Node> children = new ArrayList<>(); - an Array to store children
• int xPos, yPos; - coordinates of the node in cartesian space

---

Task 3.2 Add new nodes and their children to the code

---

Doing DFS

Starting at node S - we want to continue to explore the graph until we reach our destination (node G). We want to print out each node as we visit it.

---

Task 3.3 Do the DFS to find a path.

Hint: think about expanding the Node class, possibly with methods that allow you to test if a node has been explored, mark a node as explored and return the first unexplored child node or, if no children exist, return the parent.

---

Getting the robot to follow the path

Using the navigator class, have the robot trace out the path.

---

Task 3.4 Get the robot to trace out the path to the Goal. Note: goTo() is a non-blocking method - it will return immediately so you need to give your robot time to move.

---

In this lab, you have learned about how the Navigator class works and how we can set up a simple data structure to store information about an environment and use a DFS to find a path through it.

If you have finished this lab, move onto the final task.