예제 #1
0
  public void run() {
    System.out.println("Watching stalls");

    while (!Thread.interrupted()) {
      index = 0;
      for (Motor m : motors) {
        newSpeed = m.getActualSpeed();
        if (m.isRotating()) {
          if (newSpeed < limit) {
            if (oldSpeed[index] > newSpeed || wasStalled[index]) {
              if (oldSpeed[index] > newSpeed) {
                Sound.beep();
              }
              stalled = true;
              m.flt();
              wasStalled[index] = false;
            }
            wasStalled[index] = true;
          }
        } else {
          oldSpeed[index] = 0;
        }
        oldSpeed[index] = newSpeed;
        index++;
        // TODO: this will miss a chunk abut once every 20-50 (actually more, this probably takes
        // at least 10ms)
        try {
          Thread.sleep(95);
        } catch (InterruptedException e) {
        }
      }
    }
  }
예제 #2
0
  public void run() {
    motor_left.resetTachoCount();
    motor_left.regulateSpeed(true);
    Movement.motor_left.smoothAcceleration(true);
    int previousCommandCount = -1;

    while (true) {
      if (Movement.getCommandCount() == previousCommandCount) {
        try {
          Thread.sleep(10);
        } catch (InterruptedException e) {
        }
        continue;
      }

      previousCommandCount = Movement.getCommandCount();
      setToAngle(ControlCentre.getTargetSteeringAngleLeft());
      int new_angle = getToAngle();
      if (new_angle < 10) LCD.drawString("  ", 8, 1);
      else if (new_angle < 100) LCD.drawString(" ", 9, 1);
      LCD.drawString(Integer.toString(new_angle), 7, 1);
      LCD.drawString("R", 11, 1);

      int cur_angle = getCurrentSteeringAngle();
      double delta = new_angle - cur_angle;
      final double C = Movement.rotConstant;
      double turn_angle = 0;

      if (Math.abs(delta) < thresholdAngle / 2.0) {
        continue;
      } else if (Math.abs(delta) >= thresholdAngle / 2.0 && Math.abs(delta) < thresholdAngle) {
        delta = thresholdAngle * delta / Math.abs(delta);
      }
      setCurrentSteeringAngle((int) (cur_angle + delta) % 360);

      if (delta != 0 && Math.abs(delta) < 180) {
        turn_angle = C * delta;
      } else if (delta >= 180 && delta < 360) {
        turn_angle = -C * (360 - delta);
      } else if (delta <= -180) {
        turn_angle = C * (360 + delta);
      } else {
          /* No turning needed */
        continue;
      }

      motor_left.rotate((int) Math.round(turn_angle));
    }
  }