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;
}
