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