Climb

[mebrahimaleem]

Need a subsystem and command for climbing

[ProgrammingSR]

There are two pieces of hardware for climb: the winding motor and the locking servo
To climb:

1. Windup very slightly to allow servo lock to release
2. Release servo lock
3. Unwind to desired position
4. Engage servo lock
5. Align robot to correct position (maybe use driveToPose command for driver ease)
6. Drive forwards at a reasonable speed
7. Windup to lift robot

The motor cannot unwind while the servo lock is engaged
The motor can always windup

Before using the elevator, steps 1-4 need to run to clear the elevator (maybe)