Auto Shifting

[ianof]

Add automatic shifting with a manual override

[BenBernardCIS]

This should be part of DriveWith2Joysticks and DriveTrain.

• [ ] Add an autoShift method to DriveTrain
• [ ] Change DriveWith2Joysticks.execute() so that it first calls autoShift, then tankDrive
• [ ] The autoShift method should work as a state machine with the following rules:
• If Shifter is set to LOW, and drivetrain speed (based on encoders) is faster than SHIFT_UP_THRESHOLD, and no shift has occurred in the last SHIFT_DELAY milliseconds, shift up (using the Shift command)
• If Shifter is set to HIGH, and drivetrain speed (based on encoders) is slower than SHIFT_DOWN_THRESHOLD, and no shift has occurred in the last SHIFT_DELAY milliseconds (using the Shift command)

SHIFT_DELAY should start at around 250ms, but we can tune that as needed. SHIFT_UP_THRESHOLD should be higher than SHIFT_DOWN_THRESHOLD, but other than that the values should be determined at Falmouth.

[BenBernardCIS]

We'll need to tune the thresholds and delays at Falmouth, but it's all working.