Drive-straight and anchor strategies should have independent PID parameters

[wxb1ank]

While testing the teleoperated driving code at PAST, Alex and I realized that DriveStraightDriveStrategy and AnchorDriveStrategy should have different PID parameters from each other. Until that point, we'd been solely tuning the anchoring PID parameters, and while they are now in a good place, the P term is far too high to be reused with drive-straight; we noticed a heavy wobble when using drive-straight for more than a few seconds, which was kind of funny but is definitely not ideal.

I see two general solutions to this problem.

1. Give DriveStrategyDriveStrategy and AnchorDriveStrategy each their own PID controller object

This is what we had originally before I moved the PID controllers to GyroSubsystem. My primary reasoning for such a move was that the process variable of both drive strategies is the output of GyroSubsystem::getYaw, which necessitates calling pidController.enableContinuousInput(-180, 180) when constructing the PID controller; I figured that, to ensure this is configured for both drive strategies, this code should be located in a central place, and GyroSubsystem made sense as this code is gyro-centric.

If we return to the original design by storing a PIDController instance variable in each drive strategy, then, to avoid duplication (which allows inconsistency in behavior), it may be best to do one of the following:

1. In each drive strategy, store a PIDController subclass that makes the call to PIDController::enableContinuousInput in its constructor;
2. In each drive strategy, store a PIDController from a factory method that makes the call to enableContinuousInput;
3. Extend both drive strategies from a common superclass that contains a PIDController subclass and makes the call to enableContinuousInput and allows setting the setpoint; or
4. Store the PID controller object only in DriveStraightDriveStrategy and have AnchorDriveStrategy extend it.

One disadvantage of approach 2 is that resetting the PID controller, zeroing the setpoint, and zeroing the gyro's yaw cannot realistically be combined in a single operation, even though one nearly always wishes these operations to occur simultaneously.

Approach 4 has multiple disadvantages specifically:

1. A mechanism is required for AnchorDriveStrategy to configure the PID parameters and set the setpoint; and
2. As the enableContinuousInput call is no longer in a general factory method or subclass of PIDController, it must be duplicated if we use a PID controller with a yaw-based process variable in any future applications.

2. Adapt the existing GyroSubsystem interface

The GyroSubsystem interface already:

1. Makes the PIDController::enableContinuousInput call in its constructor;
2. Provides an API to set the setpoint; and
3. Combines resetting the PID controller, zeroing the setpoint, and zeroing the yaw in a single operation.

To adapt this interface to our new needs, we could add a method to set the PID terms like so:

public setPidTerms(final double p, final double i, final double d) {
    this.rotationalPid.setPID(p, i, d);
}

We do have to be careful about adding so much PID-related functionality directly into GyroSubsystem, though—at what point does it merely become a PIDController that happens to also have a gyro interface? As such, it may be best to take approach 1 from the previous solution.

---

What does everyone think?

(I might convert this into a discussion eventually.)

[vchen04]

1. Give DriveStrategyDriveStrategy and AnchorDriveStrategy each their own PID controller object

I'd prefer to use this solution for the reason you mentioned—we would be giving the GyroSubsystem, which can be used for more than just PID, too many PID-related responsibilities, which would make our organization a bit murky. A better organized approach would be to simply put PID-related code directly in the algorithms that use it, such as the DriveStrategy classes in this case, possibly with a PIDDriveStrategy abstract class or superclass that our DriveStraightDriveStrategy and AnchorDriveStrategy can extend (this is the "common superclass" solution). I like this approach because it both organizes our code well and limits repetition.

[wxb1ank]

1. Give DriveStrategyDriveStrategy and AnchorDriveStrategy each their own PID controller object

I'd prefer to use this solution for the reason you mentioned—we would be giving the GyroSubsystem, which can be used for more than just PID, too many PID-related responsibilities, which would make our organization a bit murky. A better organized approach would be to simply put PID-related code directly in the algorithms that use it, such as the DriveStrategy classes in this case, possibly with a PIDDriveStrategy abstract class or superclass that our DriveStraightDriveStrategy and AnchorDriveStrategy can extend (this is the "common superclass" solution). I like this approach because it both organizes our code well and limits repetition.

I implemented this solution in a commit in the vision branch while working with Limelight rotational PID. I should've just switched to master first, but as the vision subsystem is nearly complete at this point, I think we might as well wait until vision is merged back into master.

[SomeoneMysterious]

Vision has been merged into master for a bit, closing this issue.