Position Control

[OccultSlolem]

Parameters:

• If FMS has given a position control color, than we know that stage 3 is at capacity and we need to do position control (ie we can use one button for both position AND rotation control)
• Keep spinning until color sensor reads the same color as the position determined by the FMS

if(PositionControlColor.length() > 0) { //FMS has distributed a position (ie position control is unlocked)
    PositionControl();
}

[OccultSlolem]

Awaiting further discussion on 2/3

[OccultSlolem]

Merged into #17