Here is the method in the Drivetrain subsystem to drive straight.
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