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;
}
/**
* Builds a controller for the given motors.
*
* @param statusPing : a observable. On each signal of this observable, the status (deadzone) will
* be checked
* @param motors
*/
public SMMotorHandler(Observable<Signal> statusPing, NXTRegulatedMotor... motors) {
this.motorblock = new MotorBlock(motors);
// a motor is considered 'stalled' if the current angle is off by 5
// degrees for over 200ms
motorblock.setStallThreshold(ANGLE_STALL_THRESHOLD, TIME_STALL_THRESHOLD);
motorblock.calibrate(EXTREMUM_TO_CLOSED);
notifyObservers(new MovedToSignal(currentState));
// start queue thread
thread = new Thread(new QueueHandler());
thread.start();
// bind "deadzone" detector to the clock
statusPing.addObserver(new PositionChecker());
}
/** Checks the motos position. When it is in the deadzone, a 'Door open' is added to the queue */
public void checkStatus() {
if (turning) {
return;
}
try {
Position p = motorblock.getPosition(OPEN_LIMIT_BEFORE_DEADZONE, CLOSED_LIMIT_BEFORE_DEADZONE);
if (p != currentState.pos) {
setState(new Command(p, "manual"));
}
} catch (IllegalStateException e) {
addCommand(new Command(Position.OPEN, "deadzone"));
}
}