We are going to want to program the robot to follow precise paths autonomously and to do that we are going to use a technique called Pure Pursuit which we will cover later. In order for this technique to work, however, we need a way to keep precise track of the robot position. To do this we will use the Navigator class.
Since the Navigator class requires access to the DriveSubsystem’s motor encoders, we will need to create our instance of the Navigator within the DriveSubysystem. Add the following variable to your DriveSubsystem class
|
1 |
private Navigator m_navigator; |
Now, in the constructor of your Drive Subsystem add the following:
|
1 2 3 |
m_navigator = new Navigator(m_leftEncoder, m_rightEncoder); m_navigator.reset(90, 100, 100); m_navigator.setTicksPerFoot(DriveSubsystem.k_ticksPerFoot); |
And define k_ticksPerFoot as:
|
1 |
public final static double k_ticksPerFoot = 1; |
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.
The setTicksPerFoot function sets the conversion factor that will convert encoder ticks into feet.
Note that we are temporarily setting the k_ticksPerFoot to 1 now since we do not yet know the conversion factor that will convert the encoder ticks to feet.
Before we continue, build and run your program. When you do so, you will see that it takes longer to initialize the robot. This is because the initialization of the gyroscope takes some time. While this initialization is happening you will see the following output in the Terminal window:
|
1 2 3 4 5 6 7 8 9 10 |
4002:MotorBase(3):MotorBase() 4027:DriveSubsystem(3):DriveSubsystem() 4033:Navigator(1):Wait for initialization... 5038:Navigator(1):Waiting for navigator: state = 0 6042:Navigator(1):Waiting for navigator: state = 0 7046:Navigator(1):Waiting for navigator: state = 0 8049:Navigator(1):Waiting for navigator: state = 0 9053:Navigator(1):Waiting for navigator: state = 0 10057:Navigator(1):Waiting for navigator: state = 0 11060:Navigator(1):Waiting for navigator: state = 1 |
Once you see the line showing state = 1, the initialization is complete. Note that this process will happen each time you deploy new code to your robot.
Occasionally the Navigator will not initialize properly and the state never switches to 1. If this happens, simply kill your program and restart it.
Now we need to compute the conversion factor that will convert ticks to feet. To do this we will create a command that will drive the robot forward at 50% speed for 7000 encoder ticks and we will use the Navigator class to measure this distance.
Since we will be needing access to the Navigator instance within this new command, create a function in your DriveSubsystem class to return the Navigator instance:
|
1 2 3 |
public Navigator getNavigator() { return m_navigator; } |
Now create a CalibrateDistanceCommand that uses the DriveSubsystem:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 |
public class CalibrateDistanceCommand extends CommandBase { private final DriveSubsystem m_subsystem; private final Navigator m_navigator; public CalibrateDistanceCommand(DriveSubsystem subsystem) { Logger.log("CalibrateDistanceCommand", 3, "CalibrateDistanceCommand()"); m_subsystem = subsystem; m_navigator = m_subsystem.getNavigator(); // Use requires() here to declare subsystem dependencies addRequirements(m_subsystem); } |
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 using the Navigator’s reset function as follows:
|
1 |
m_navigator.reset(90, 0, 0); |
Now in your execute function, use the Navigator’s getPos function to get and log the current robot position as follows:
|
1 2 3 |
NavigatorPos pos = m_navigator.getPos(); Logger.log("CalibrateDistanceCommand",1,String.format(",%.2f,%.3f,%.3f", pos.yaw, pos.x, pos.y)); |
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 set the appropriate value for your k_ticksPerFoot.
In my case, the robot drove for 7050.2 ticks at a distance 5.85 feet. So in my DriveSubsystem I changed the value of k_ticksPerFoot as follows:
|
1 |
public final static double k_ticksPerFoot = 7059.2 / 5.85; |
Once you have done this, all further calls to the Navigator’s getPos function will return the robot’s position in feet.