Using PID to Drive Straight and A Measured Distance

Back in the Recycle Rush days we figured out that having two PID controllers in one command is very challenging. The solution was to have one "P" controller in the Drivetrain Subsystem to correct the robot driving straight, and to have a full PID controller in a command to drive a distance.
​
Here is the method in the Drivetrain subsystem to drive straight.

Note: the HDrive call essentially simplifies to a call to arcade drive.

The real magic happens in the command. In the usePIDOutput we do a bit of massaging on the output value.

• First we constrain output between negative maxspeed and maxspeed.
• Then we increase any output value less than .4 to .4 as that seemed to be the minimum speed to make the robot move.
• Then we set an acceleration rate with a time dependent function so the wheels don't spin out when the PID initially becomes active and calclulates a large error