示例#1
0
 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;
 }
示例#2
0
  /**
   * 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());
  }
示例#3
0
  /** 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"));
    }
  }