private void turnTo(Command command, boolean allowTurnBack) { turning = true; int turn = command.pos.getPos(); motorblock.setSpeed(900); motorblock.rotateTo(turn, false); // if the motor is not in the projected area after turning, undo the // turn motorblock.flt(); if ((allowTurnBack && (motorblock.getTachoCount() > turn + ALLOWED_TACHO_MARGIN || motorblock.getTachoCount() < turn - ALLOWED_TACHO_MARGIN))) { turnTo(currentState, false); throw new IllegalStateException("turnto failed, motorblock turned back"); } turning = false; }