Path Following

Now that we know how to create paths, let’s see if we can get the robot to follow one.

First we need to make some changes to our DriveSubsystem. We are going to use the PurePursuit class to control the motion of the robot so add a PurePursuit variable:

Now the constructor for PurePursuit requires a function that will set the speed of the left and right wheels in feet/second, so we need to create that:

Here we are computing the speed as percentage of the max speed by multiplying the speed in feet/second by the ratio of the ticks per foot to the max speed. We can then call our existing setSpeed function.

There is one other change we need to make in addition to creating this setSpeedFPS function. The PurePursuit path follower runs in it’s own thread and will make calls to set the left and right motor speeds via the function you just created. Since these calls will be from a different thread than your normal calls to setPower and setSpeed, we need to enclose any access to the motor classes within a synchronized command. For example, your setPower function should look something like:

Where you have declared m_lock something like:

You should have a similar synchronized command in your setSpeed and any other place in your code that you access the instances of your motors.

We are now ready to create an instance if PurePursuit in the constructor of our DriveSubsystem:

Of course we must define k_purePersuitUpdateRate:

Note the call to enableLogging. This allows us to log details about the followed path to a file on the Raspberry Pi, which will allow us to study how well the robot is following the path.

Finally we will need a few functions to control the path following from our commands:

Before we create our first follow path command, we need to do one more thing. The Pure Pursuit path following has a number of tunable parameters. While it is possible to specify a different set of parameters for each path, it is useful to set up a set of defaults that are used for each new path we create. You do this in the RobotContainer’s constructor by adding the following. Be sure to place these lines before the call to configureButtonBindings().

Where we have defined the constants as:

Let’s take a look at what each of these values mean. See the documentation for PathFinder for further information.

  • Look Ahead Time – This is the time that Pure Pursuit looks ahead on the path to find the point at which to steer. We will use a default of 0.75 seconds along the path.
  • Minimum Look Ahead Distance – If the distance computed by using the Look Ahead Time is less than this minimum, then this value will be used instead. We will set this minimum to default to 0.6 feet.
  • Maximum Search Time – On Each update, the path will be searched for the point on the path that is closest to the robot’s current position. To avoid problems when the robot loops back on it’s own path, this value specifies the maximum time to look ahead for the closest point. We will set this to default to 0.25 seconds along the path.
  • Minimum Speed – This value specifies the minimum speed for the robot that is needed to keep the robot from stalling. We will set this value 0.50 feet/second.
  • Extended Look Ahead – This value specifies the amount to extend the path for navigation purposes if the isExtended flag is set to true on the call to loadPath. Note that this extension is used for navigation purposes only and the robot will still stop at the final ending point. We will set this to default to 0.75 feet.
  • Curvature Adjust – This value is used to force the robot to more (or less) aggressively steer toward the target point. A value of 1 will use the curvature that is computed by the Pure Pursuit algorithm. A value greater than 1 will cause the robot to steer more aggressively towards that point and a value less than 1 will cause the robot to steer less aggressively. We set this to the default of 1.0.

We are now ready to create our first path following command. For this command we will instruct the robot to drive forward 5 feet. The first thing to do is to create the path in the PathPlanner program. Graphing the position, velocity and acceleration we see the following:

The structure of all of your path following commands will be similar. Below is my example of a command to drive forward 5 feet. Note that I am placing all of my pursuit commands into a purePursuit subfolder of commands just to keep them organized.

Lets take a look at a couple of features of this command. First we have the following lines which defines the path.

These numbers come directly from the PathPlanner program. Note, however, you do not need to type these in. If you click the Copy button in PathPlanner, these lines will be copied to the clipboard and you can just paste them into your program.

In our constructor we create the path by calling the computePath function of the Pathfinder class.

In our initialize function we reset the robot to it’s starting position (0,0) and starting yaw (angle) of 90 degrees. Note that if we later make a command which consists of multiple path following commands we only want to do this reset in the first of the set.

Also in our initialize function we start the path following by calling the startPath function of our DriveSubsystem.

Note that the first Boolean parameter controls whether the robot will move forward (false) or backwards (true). We want to go forward so this is set to false. Setting the second Boolean parameter to true will help keep the robot from turning as it approaches it’s destination. The last parameter controls whether the navigator will be automatically reset to the starting position and angle for the path. We always want to set this to true for the first path. If we have it follow multiple paths, we will set it to false for the subsequent paths.

In the end function we end the path following by calling the endPath function of our DriveSubsystem.

Finally in the isFinished function we return whether the path is complete by calling the isPathFinished function of our DriveSubsystem.

Now connect this command to a button (using whenPressed) and run your program. When you press the button, the robot should drive forward 5 feet and then stop.

Now try creating a more complicated path that drives the robot from (3,0) at 90 degrees to (-3,0) at -90 degrees. You can construct the path using a single Bezier curve like this:

Or you can connect two Bezier curves like this:

Note that if we need the robot to go through the point (0,3) then it is easier to do it with 2 curves rather than one, although it would still be possible with one by changing the Bezier control points.

Create a new command called SemiCircleCommand that implements this path. The easiest way is to copy the Drive5ftCommand and replace the path with your new one. Remember that you can use the Copy button in the PathPlanner and paste that code into your command. When you have finished run your program and verify that the robot drives the correct path.

Now before we move on, I want to introduce you to a tool that will let you examine the path the robot took in detail. The way we have configured PurePursuit, it will log detailed information about the path following to a logs folder in the home directory of the pi.

To examine this data, we will first need to upload the log file to our compute. For this we will us the FTP program FileZilla. Then connect to the pi by selecting the Pi.AP option from the dropdown shown below:

Create a logs subfolder in your robot project folder and then in the left Local Site pane navigate to that folder. In the right Remote Site pane, navigate to the logs folder as shown below:

Click on the Last modified header of the right pane to show the most recent file first. Then double click on the most recent file to copy it to the logs folder on your computer.

Then right click on the file in the left pane and choose Open. This will open the file in LibreCalc.

First we will compare the Ideal left and right velocities to the Actual left and right velocities. Select the columns D, E, F, and G as shown.

Then create a chart choosing Line/Lines Only options:

This will produce a graph showing the Ideal and Actual velocity of both wheels and we can see how closely the velocities are being matched.

Finally, we can plot the Actual path to the Ideal path to see how well they match as well. Do do this in LibreCalc, we will first need to select all of the Ideal x and y data (columns J and K) and past it onto the end of the Actual x and y data (columns H and I) as shown:

Then select columns H and I and create a chart, this time choosing XY (Scatter):

As a final exercise for this section, create a command to follow the following path:

Next: Final Challenge