Exemplo n.º 1
0
  public void setSpeed(OutputStream out, byte[] buf, int len) {
    int leftSign = 1;
    int rightSign = 1;
    int p = 2;

    if (buf[p] == '-') { // if input is '-', move right one place
      leftSign = -1;
      p++;
    }
    int leftPwr = 0;
    while ('0' <= buf[p] && buf[p] <= '9')
      leftPwr = leftPwr * 10 + (buf[p++] - '0'); // accumulate the decimal value
    if (leftPwr > H_MAX_POWER) leftPwr = H_MAX_POWER; // stay in range
    leftPwr = leftSign * leftPwr;

    // assert(buf[p] == ',');
    p++;

    if (buf[p] == '-') {
      rightSign = -1;
      p++;
    }
    int rightPwr = 0;
    while ('0' <= buf[p] && buf[p] <= '9') rightPwr = rightPwr * 10 + (buf[p++] - '0');
    if (rightPwr > H_MAX_POWER) rightPwr = H_MAX_POWER; // stay in range
    rightPwr = rightSign * rightPwr;

    // assert(buf[p] == 0); // the buffer is null-terminated
    p++;

    leftMotor.setPower((leftPwr * 16) / H_MAX_POWER);
    rightMotor.setPower((rightPwr * 16) / H_MAX_POWER);
    writeCRLF(out, "d");
  }
Exemplo n.º 2
0
  public void motorsOff(OutputStream out, byte[] buf, int len) {
    leftMotor.setPower(0);
    rightMotor.setPower(0);

    writeCRLF(out, "z");
  }
Exemplo n.º 3
0
  public static void run(String args[]) {
    try {
      System.out.println("WheelWatcher WW-01");

      // Get the user interface objects
      Display display = IntelliBrain.getLcdDisplay();
      AnalogInput thumbwheel = IntelliBrain.getThumbWheel();

      // Get the servo object and give it a Motor interface by encapsulating it
      // in a ContinuousRotationServo object.
      Motor motor = new ContinuousRotationServo(IntelliBrain.getServo(3), true);

      motor.setPower(1);
      motor.setPower(0);
      motor.setPower(-1);
      Thread.sleep(2000);

      // Get a shaft encoder object and initialize it with the two digital inputs it uses.
      IntelliBrainShaftEncoder encoder = IntelliBrain.getShaftEncoder(1);
      encoder.initialize(IntelliBrain.getDigitalIO(11), IntelliBrain.getDigitalIO(10));

      int previousCounts = 0;
      int power = 0;
      int rpm = 0;
      long time = System.currentTimeMillis();
      while (true) {
        // Calculate RPM (Revolutions Per Minute)
        // RPM = countsThisIteration * iterationsPerMinute / countsPerRevolution
        // countsThisIteration - current encoder counts minus previous count
        // iterationsPerMinute = 600 - ten loops per second
        // countsPerRevolution = 128 - fixed by design of sensor/codewheel
        int counts = encoder.getCounts();
        display.print(1, "RPM: " + ((counts - previousCounts) * 600) / 128);
        previousCounts = counts;

        // Read the thumbwheel and scale the value to set the motor power.
        int velocity = (thumbwheel.sample() - 512) / 31;
        while (velocity == 3 || velocity == 4) {
          if (velocity == 3) velocity = (thumbwheel.sample() - 512) / 31;
          else velocity = -(thumbwheel.sample() - 512) / 31;

          // counts and rpm logic
          counts = encoder.getCounts();
          previousCounts = counts;
          rpm = ((counts - previousCounts) * 600) / 128;
          display.print(0, "RPM: " + rpm);

          // governer logic
          power += (velocity - (rpm / 16.25));

          // limiter
          if (power > 16) power = 16;
          if (power < -16) power = -16;
          display.print(0, "Power: " + power);

          // set power
          motor.setPower(power);

          time += 100;
          Thread.sleep(time - System.currentTimeMillis());
        }
        motor.setPower(velocity);
        display.print(0, "Power: " + velocity);

        // Calculate sleep time so the next iteration starts 1/2 second after the
        // the previous one.
        time += 100;
        Thread.sleep(time - System.currentTimeMillis());
      }
    } catch (Throwable t) {
      // Display a stack trace if an exception occurs.
      t.printStackTrace();
    }
  }