Path Following

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

We will use the Navigator class to keep track of the robot’s position. Initially, the Navigator class will return the robot’s position in encoder ticks. Since this is not that useful, the first thing we need to do is to calibrate it to return the position in feet.

We will create a command that will drive the robot forward at 50% speed for 7000 encoder ticks. We will be using the Navigator class to measure this distance.

In your RobotContainer class, create an instance of the Navigator as follows. Note that we are using the getInstance function to get the current navigator instance instead of using the new directive.

Then in RobotContainer’s constructor, add the following code:

The setUpdateRate function sets the rate at which the position is updated. For the current Arduino serial connection setting this faster that 20 updates/sec (i.e. 0.05 seconds) is pointless.

The invert function lets us specify whether the sense of the yaw, and the x, and y coordinates are inverted. For the way this module is currently wired, you will need to invert both the x and the y but not the yaw.

The reset function lets up set the current yaw and the x and y coordinates. We set them here so that when the robot starts it is in a known state. Individual commands may need call this reset as well.

Now create a CalibrateDistanceCommand that uses the DriveSubsystem and takes an instance of Navigator in it’s constructor:

Then in your initialize function start the robot moving forward at 50% speed and reset the Navigator to the coordinate (0,0) at an angle of 90 degrees as follows:

Now in your execute function, use the Navigator’s getPos function to get and log the current robot position as follows:

Finally set your isFinished function to return true when the y coordinate equals or exceeds 7000.

Then mark the starting position of the robot and run your program and measure the distance from the start to the end point. Note that your robot may not drive exactly straight so you want to measure the diagonal. Then compute the number of encoder ticks that the robot moved by computing the hypotenuse of the triangle represented by the ending x and y coordinates. Finally create a public constant in your DriveSubsystem which defines the ticks/foot.

In my case, the robot drove for 7050.2 ticks at a distance 5.85 feet. So in my DriveSubystem I added the line:

Then set the conversion factor for the Navigator by calling the setTicksPerFoot function in the constructor of your RobotContainer:

Once you have done this, all further calls to the Navigator’s getPos function will return the robot’s position in feet.

Next 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:

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.25 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.

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