PID for Target Position

As part of Curriculum challenge ~ Moving to a target position, you wrote your own PD controller - something that decided how to drive based on

1. The Proportional error between you and the target (distance)
2. The Derivative (rate of change) of the error between the robot and the target (speed)

Rather than rewrite that code over and over, we have a highly-tested and reliable PD controller that you can use.

Setting up the PIDManager

To get it, we will get a PIDManagerFactory, and we'll use that to create a PIDManager, which is what we actually will use in your class (ask a mentor if you want an explanation for this two-step process). Let's modify the DriveToPositionCommand as follows:

    @Inject
    public DriveToPositionCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose, PIDManagerFactory pidManagerFactory){
        this.drive = driveSubsystem;
        this.pose = pose;
        this.pid = pidManagerFactory.create("DriveToPoint");
    }
 

We can also tell the PIDManager what the acceptable "finishing conditions" are:

        pid.setEnableErrorThreshold(true); // Turn on distance checking
        pid.setErrorThreshold(0.1);
        pid.setEnableDerivativeThreshold(true); // Turn on speed checking
        pid.setDerivativeThreshold(0.1);
    }
 

Let's also give it some P and D values - though you should have better values from your own testing rather than the ones I picked out of a hat here:

        pid.setEnableErrorThreshold(true); // Turn on distance checking
        pid.setErrorThreshold(0.1);
        pid.setEnableDerivativeThreshold(true); // Turn on speed checking
        pid.setDerivativeThreshold(0.1);
        
        // manually adjust these values to adjust the action
        pid.setP(0.5);
        pid.setD(2);
    }
 

Finally, the PIDManager holds onto some information that needs to be reset each time the command is run, just like how you probably used a variable to hold onto the last position of the robot. As a result, we'll call the PIDManager's reset() function in initialize:

 
    @Override
    public void initialize() {
        // If you have some one-time setup, do it here.
        pid.reset();
    }
 

Using the PIDManager to get to our target

Now to use this thing! Do something like the code below (it may be slightly different depending on how you wrote your DriveSubsystem):

 
    @Override
    public void execute() {
        double currentPosition = drive.getPosition();
        double power = pid.calculate(goal, currentPosition);
        drive.tankDrive(power, power);
    }
 

Our execute() is so short now! And we can do something similar for isFinished():

 
    @Override
    public boolean isFinished() {
        return pid.isOnTarget();
    }
 

All that positional error checking, and speed checking, are just done automatically for you as long as you've enabled them and set some thresholds, which we did in our constructor. What a time savings!

There's all sorts of features the PIDManager brings to the table that we haven't even used yet:

• You can set a time threshold, so the robot has to be "on target" for a certain time duration before it reports that it is done
• You can modify the P, I, and D values while the robot is running using your laptop without recompiling the code

And most importantly, there are 15+ tests that run against the PIDManager every time the robot code is built, making sure that this critical piece of code is in great shape; you'll still need to test your class's overall logic, but you can feel confident that the PIDManager is behaving well.

Fixing the test cases

You'll notice that by following these instructions, your tests don't compile anymore. That's because we added a parameter to DriveToPositionCommand's constructor and your tests are all calling new DriveToPositionCommand directly with the old parameters, which don't work anymore. We can fix this by updating the tests to use dependency injection!

In the main robot code, we didn't need to update any references to DriveToPositionCommand because the robot framework is already set up for dependency injection. Our tests need a little bit more work though. If we want to be able to create our command using dependency injection, we need to write a little bit of glue code.

Find the CompetitionTestComponent class. This class is what the unit tests use for dependency injection. You can add a method to this class to expose the DriveToPositionCommand:

 
    public abstract DriveToPositionCommand getDriveToPosititonCommand();
 

Our dependency injection framework uses this as an instruction to create a method that finds everything the DriveToPositionCommand needs automatically. With this helper added, you can update the broken tests to create command instances using dependency injection:

Replace instances of new DriveToPositionCommand(...) with this.getInjectorComponent().getDriveToPosititonCommand() in the tests. getInjectorComponent() is available from any test, and you can use it to ask for anything you define in the CompetitionTestComponent.

Why would you want to make a change like this? Now that you're using injection, you could change the constructor on DriveToPositionCommand to add any new dependencies you want, and you won't have to update your tests or anything else that depends on it!

Go ahead and run the LinearTestVisualizer to make sure everything works, and once you're ready, let's move on to the next lesson and upgrade rotation.

note: TurnLeft90DegreesCommand can be extended from the more general case DriveToOrientationCommand