# Navigating a simple maze

## Video

LEGO + Arduino robot navigating the simple maze

## Objective

Now that we have a grasp of how to make the robot move, we can use that understanding to complete a task. For this section we are going to make the robot navigate a very simple maze. We will be telling the robot exactly where to go, so there is no need to worry about how it will detect the course.

Layout of the maze we will be traversing

## The Plan

The first step in programing a robot to follow a path is to plan the path the robot will take to navigate the maze. Since this maze has just one path, the planning has already been done for you. The path the robot will take is out lined below.

simple maze drawing

However, in order to follow this path, we will need to find out how long it takes for the robot to turn 90° to the left and right at speed 100, and how long it takes the robot to travel 1 in. at speed 100. Getting the time to travel 1 in. is the simplest, so we will start with that, then move on to finding the time to make the turns.

### Distance Traveled vs Time

Since we are looking for a ratio of seconds per inch, we can find it by taking the inverse of inches per second (i.e. speed). In either case, to find the relationship, you need to know the distance traveled and the time to travel of at least one sample to calculate the relationship. Since we already know that we can control the time that the robot will run for, it is a very simple matter to just program it to drive for 5 seconds, then measure the distance traveled. You can either write your own program to drive straight forward at speed 100 for 5 seconds or you can use the following code.

 ```#pragma config(CircuitBoardType, typeCktBoardUNO) #pragma config(PluginCircuitBoard, typeShieldDFRobotMotor) #pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0) #pragma config(Motor, motor_5, rightServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl5, dgtl4) #pragma config(Motor, motor_6, leftServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl6, dgtl7) //*!!Code automatically generated by 'ROBOTC' configuration wizard  !!*//   task main() { wait1Msec(1000); //pause code execution for 1000ms (1 second)   motor[leftServo] = 100; motor[rightServo] = 100; wait1Msec(5000); motor[leftServo] = 0; motor[rightServo] = 0; }```

Now download the program to the robot and set it up so that the point where the wheel touches the driving surface is next to a yard/meter stick or a tape measure. Now turn it on, wait for the robot to stop, and record the distance traveled. We recommend that you do this 2 more times to get an average value.

setup to measure the distance traveled

If you take the average distance traveled and divide it by the time (5 seconds), you will get the distance per second. This can be useful if you know how long you will be traveling and not the distance, but we know the distance we need to travel without the time it will take to get there. To find a relationship that will get us that second number, we take the time (5 seconds) and divide it by the number of inches traveled. So for example, if you went 8 inches in five seconds, your speed (inches/second) would be 8/5 or 1.6 and the ratio we are looking for (seconds/inch) would be 5/8, or .625.

Once we have this ratio, we can take some distance, say 9 inches, and calculate the time needed to travel that distance by multiplying the desired distance by the second ratio we found.

### Adjusting Turning Time

While the process of calculating the time for driving straight for some distance is fairly simple, the physics involved with turning make it a little more complicated. Since we only need to make 90° turns, we will just create a program to make a left point turn and adjust he timing until the turn is sufficiently close to 90°. Then we just repeat the process for a right point turn. From our testing, the following code should get you close to a 90° point turn to the left.

 ```#pragma config(CircuitBoardType, typeCktBoardUNO) #pragma config(PluginCircuitBoard, typeShieldDFRobotMotor) #pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0) #pragma config(Motor, motor_5, rightServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl5, dgtl4) #pragma config(Motor, motor_6, leftServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl6, dgtl7) //*!!Code automatically generated by 'ROBOTC' configuration wizard  !!*//   task main() { wait1Msec(1000); //pause code execution for 1000ms (1 second)   motor[leftServo] = -100; motor[rightServo] = 100; wait1Msec(500); motor[leftServo] = 0; motor[rightServo] = 0; }```

## Programming

Now that we have the turning times and an idea for the forward times, we can start to make the program to navigate the maze.

### Defining the Functions

Since navigating the maze uses just 4 simple behaviors, we are going to use functions to make the programming easier.

#### Driving Forward

One of the behaviors is to drive forward at speed 100 for some amount of time. So let's define a function that will set both motors to speed 100 and pause the code execution for a passed time parameter. It is possible to make the function receive the desired distance and calculate the time based on the time/distance ratio calculated earlier, but by using time, fine tuning is much easier.

 ```// drive forward at speed 100 for the passed time in milliseconds void DriveForward(int time) { motor[leftServo] = 100; motor[rightServo] = 100; wait1Msec(time); }```

#### Turning Left

Now that we can go straight, we will eventually need to turn. Conveniently, every turn is a 90° point turn to the right or left. Let's start with the left turn. Take the time you got for your left turn and place it in the wait1Msec() command.

 ```//make a 90 degree pivot turn to the left at speed 100 void TurnLeft() { motor[leftServo] = -100; motor[rightServo] = 100; wait1Msec(470); //you will probably need to adjust time for your robot }```

#### Turning Right

We will also need to turn right so let's make a right turn function and replace the turning time with the time that you calculated.

 ```//make a 90 degree pivot turn to the right at speed 100 void TurnRight() { motor[leftServo] = 100; motor[rightServo] = -100; wait1Msec(460); //you will probably need to adjust time for your robot }```

#### Stopping

Of course, once you get to the goal, you will need to stop, but you might also want to stop after each move. For this reason we are going to make a "Stop" function.

 ```//stop driving void Stop() { motor[leftServo] = 0; motor[rightServo] = 0; }```

### Putting the Behaviors together

 ```#pragma config(CircuitBoardType, typeCktBoardUNO) #pragma config(PluginCircuitBoard, typeShieldDFRobotMotor) #pragma config(UART_Usage, UART0, uartSystemCommPort, baudRate200000, IOPins, dgtl1, dgtl0) #pragma config(Motor, motor_5, rightServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl5, dgtl4) #pragma config(Motor, motor_6, leftServo, tmotorInternalHBridgeSinglePWM, openLoop, reversed, IOPins, dgtl6, dgtl7) //*!!Code automatically generated by 'ROBOTC' configuration wizard  !!*//   // drive forward at speed 100 for the passed time in milliseconds void DriveForward(int time) { motor[leftServo] = 100; motor[rightServo] = 100; wait1Msec(time); }   //make a 90 degree pivot turn to the right at speed 100 void TurnRight() { motor[leftServo] = 100; motor[rightServo] = -100; wait1Msec(460); //you will probably need to adjust time for your robot }   //make a 90 degree pivot turn to the left at speed 100 void TurnLeft() { motor[leftServo] = -100; motor[rightServo] = 100; wait1Msec(470); //you will probably need to adjust time for your robot }   //stop driving void Stop() { motor[leftServo] = 0; motor[rightServo] = 0; }   task main() { //wait 1 seconds before starting. wait1Msec(1000);   DriveForward(2207); //you will probably need to adjust time for your robot TurnLeft(); DriveForward(3862); //you will probably need to adjust time for your robot TurnRight(); DriveForward(2207); //you will probably need to adjust time for your robot TurnRight(); DriveForward(2207); //you will probably need to adjust time for your robot Stop(); }```