Get elevator extension code working

[n-arms]

The elevator should be able to extend to given positions automatically. The elevator subsystem should operate in two modes: position mode and raw input mode.

Raw input mode

• encoder readings have to be accurate and consistent across power cycles
• the elevator motor should run at the given speed
• it should never hyperextend or hypercontract

Position mode

• the elevator motor should use a simple control loop (PID with I,D=0 + a clamp to keep it relatively slow will probably work) to extend to a specific position
• see above about hyperextending

[SomewhatMay]

Hyper extension and contraction are already handled using lowAngle and highAngle values in the Elevator.java sybststem; we just have to fine tune the values.

[SomewhatMay]

PID added to elevator in branch origin/elevator-extension along with the necessary methods. Needs testing.

[SomewhatMay]

Forgot to actually check if positionMode is enabled or not when using PID in periodic()... gotta fix that tomorrow 😅 (Currently, robot will use PID regardless for elevator extension of what you set position mode to, meaning .setMotor() does nothing outside of the elevator subsystem class...)

[SomewhatMay]

Fixed it; needs testing though 😓