public void setFirstStagePos(int pos) {
        if(!brake.get()) {
            first.set(ControlMode.Position, pos);
        } else {
            first.set(ControlMode.PercentOutput, 0);