public void start(iStopCondition stopCond, boolean forward) {
    final int power = 300;
    int sensorVal, powerDiff, whiteDiff = 0;

    pidController = new PIDController(sensor.blackWhiteThreshold, 0);
    pidController.setPIDParam(PIDController.PID_KP, 1.8f);
    pidController.setPIDParam(PIDController.PID_KD, 8f);

    // LCD.clear();
    LCD.drawString("Light: ", 0, 1);

    LCD.drawString("pDiff: ", 0, 2);

    while (!stopCond.stopLoop()) {
      // whiteDiff = (sensorRef.wasWhite() ?
      // sensorRef.getChachedLightValue() - whiteRef : whiteDiff);
      sensorVal = sensor.light() - whiteDiff / 2;

      powerDiff = pidController.doPID(sensorVal);
      if (forward) {
        pilot.forward(power + powerDiff, power - powerDiff);
      } else {
        pilot.backward(power - powerDiff, power + powerDiff);
      }

      LCD.drawInt(sensorVal, 4, 10, 1);
      LCD.drawInt(powerDiff, 4, 10, 2);
      LCD.refresh();

      Thread.yield();
    }
    pilot.stop();
    LCD.clear();
  }
  public static void main(String[] aArg) throws Exception {
    String left = "Turn left ";
    String right = "Turn right";

    LightSensor light = new LightSensor(SensorPort.S3);
    final int blackWhiteThreshold = 45;

    DataLogger dl = new DataLogger("Memory10MS_C.txt");
    int lightValue;

    Runtime rt = Runtime.getRuntime();

    final int forward = 1;
    final int stop = 3;
    final int flt = 4;
    final int power = 80;

    // Use the light sensor as a reflection sensor
    light.setFloodlight(true);
    LCD.drawString("Light %: ", 0, 0);

    // Show light percent until LEFT is pressed
    LCD.drawString("Press LEFT", 0, 2);
    LCD.drawString("to start", 0, 3);
    while (!Button.LEFT.isDown()) {
      LCD.drawInt(light.readValue(), 3, 9, 0);
    }

    // Follow line until ESCAPE is pressed
    LCD.drawString("Press ESCAPE", 0, 2);
    LCD.drawString("to stop ", 0, 3);

    dl.start();
    while (!Button.ESCAPE.isDown()) {

      lightValue = light.readValue();
      if (lightValue > blackWhiteThreshold) {
        // On white, turn right
        LCD.drawString(right, 0, 1);
        MotorPort.B.controlMotor(0, stop);
        MotorPort.C.controlMotor(power, forward);
      } else {
        // On black, turn left
        LCD.drawString(left, 0, 1);
        MotorPort.B.controlMotor(power, forward);
        MotorPort.C.controlMotor(0, stop);
      }
      LCD.drawInt(lightValue, 3, 9, 0);
      dl.writeSample(rt.freeMemory());
      Thread.sleep(10);
    }

    // Stop car gently with free wheel drive
    MotorPort.B.controlMotor(0, flt);
    MotorPort.C.controlMotor(0, flt);
    LCD.clear();
    dl.close();
    LCD.drawString("Program stopped", 0, 0);
    Thread.sleep(1000);
  }
Example #3
0
  public int[] getAngledDistances(DifferentialPilot pilot, int angle, int[] prevVals) {

    // const
    int sleepTime = 350;

    // action info
    LCD.clear(0);
    LCD.drawString("Finding direction", 0, 0);

    // get central dist
    prevVals[1] += getFastMeasurement();
    LCD.drawInt(prevVals[1], 5, 1);

    // rotate, ping, wait a bit
    pilot.rotate(angle);

    // read left distance
    prevVals[0] += getFastMeasurement();
    LCD.drawInt(prevVals[0], 0, 1);

    // rotate, ping, wait a bit
    pilot.rotate(-2 * angle);

    // read right distance
    prevVals[2] += getFastMeasurement();
    LCD.drawInt(prevVals[2], 10, 1);

    pilot.rotate(angle);

    return prevVals;
  }
Example #4
0
  public static void main(String[] args) throws Exception {
    ColorHTSensor cmps = new ColorHTSensor(SensorPort.S1);
    String color = "Color";
    String r = "R";
    String g = "G";
    String b = "B";

    String[] colorNames = {
      "Red",
      "Green",
      "Blue",
      "Yellow",
      "Magenta",
      "Orange",
      "White",
      "Black",
      "Pink",
      "Gray",
      "Light gray",
      "Dark Gray",
      "Cyan"
    };

    while (!Button.ESCAPE.isDown()) {
      LCD.clear();
      LCD.drawString(cmps.getVendorID(), 0, 0);
      LCD.drawString(cmps.getProductID(), 0, 1);
      LCD.drawString(cmps.getVersion(), 9, 1);
      LCD.drawString(color, 0, 3);
      LCD.drawInt(cmps.getColorID(), 7, 3);
      LCD.drawString(colorNames[cmps.getColorID()], 0, 4);
      LCD.drawString(r, 0, 5);
      LCD.drawInt(cmps.getRGBComponent(Color.RED), 1, 5);
      LCD.drawString(g, 5, 5);
      LCD.drawInt(cmps.getRGBComponent(Color.GREEN), 6, 5);
      LCD.drawString(b, 10, 5);
      LCD.drawInt(cmps.getRGBComponent(Color.BLUE), 11, 5);
      LCD.refresh();
      Thread.sleep(INTERVAL);
    }
  }
Example #5
0
  public static void main(String[] args) throws Exception {
    LCD.drawString("waiting", 0, 0);
    USBConnection conn = USB.waitForConnection();
    DataOutputStream dOut = conn.openDataOutputStream();
    DataInputStream dIn = conn.openDataInputStream();

    while (true) {
      int b;
      try {
        b = dIn.readInt();
      } catch (EOFException e) {
        break;
      }
      dOut.writeInt(-b);
      dOut.flush();
      LCD.drawInt(b, 8, 0, 1);
    }
    dOut.close();
    dIn.close();
    conn.close();
  }
Example #6
0
  public static void main(String[] args) throws Exception {
    String connected = "Connected";
    String waiting = "Waiting...";
    String closing = "Closing...";

    while (true) {
      LCD.drawString(waiting, 0, 0);
      LCD.refresh();

      // BTConnection btc = Bluetooth.waitForConnection();
      BTConnection btc = Bluetooth.waitForConnection(0, NXTConnection.RAW);

      LCD.clear();
      LCD.drawString(connected, 0, 0);
      LCD.refresh();

      DataInputStream dis = btc.openDataInputStream();
      DataOutputStream dos = btc.openDataOutputStream();

      for (int i = 0; i < 100; i++) {
        int n = dis.readInt();
        LCD.drawInt(n, 7, 0, 1);
        LCD.refresh();
        dos.writeInt(-n);
        dos.flush();
      }

      dis.close();
      dos.close();
      Thread.sleep(100); // wait for data to drain
      LCD.clear();
      LCD.drawString(closing, 0, 0);
      LCD.refresh();
      btc.close();
      LCD.clear();
    }
  }
Example #7
0
  public static void main(String[] args) throws Exception {
    RemoteNXT nxt = null;
    int power = 0;
    int mode = 1;
    int motor = 0;
    String motorString = "Motor:";
    String modeString = "Mode:";
    String powerString = "Power:";
    String batteryString = "Battery:";
    String lightString = "Light:";
    String tachoString = "Tacho:";

    // Get the type of communications to be used
    String[] connectionStrings = new String[] {"Bluetooth", "RS485"};
    TextMenu connectionMenu = new TextMenu(connectionStrings, 1, "Connection");
    NXTCommConnector[] connectors = {Bluetooth.getConnector(), RS485.getConnector()};

    int connectionType = connectionMenu.select();

    // Now connect
    try {
      LCD.clear();
      LCD.drawString("Connecting...", 0, 0);
      nxt = new RemoteNXT("NXT", connectors[connectionType]);
      LCD.clear();
      LCD.drawString("Type: " + connectionStrings[connectionType], 0, 0);
      LCD.drawString("Connected", 0, 1);
      Thread.sleep(2000);
    } catch (IOException ioe) {
      LCD.clear();
      LCD.drawString("Conn Failed", 0, 0);
      Thread.sleep(2000);
      System.exit(1);
    }

    LCD.clear();
    RemoteMotor[] motors = {nxt.A, nxt.B, nxt.C};
    LightSensor light = new LightSensor(nxt.S2);
    while (true) {
      // Get data from the remote NXT and display it
      LCD.drawString(motorString, 0, 0);
      LCD.drawInt(motor, 3, 10, 0);
      LCD.drawString(powerString, 0, 1);
      LCD.drawInt(power, 3, 10, 1);
      LCD.drawString(modeString, 0, 2);
      LCD.drawInt(mode, 3, 10, 2);
      LCD.drawString(tachoString, 0, 3);
      LCD.drawInt(motors[motor].getTachoCount(), 6, 7, 3);
      LCD.drawString(batteryString, 0, 4);
      LCD.drawInt(nxt.Battery.getVoltageMilliVolt(), 6, 7, 4);
      LCD.drawString(lightString, 0, 5);
      LCD.drawInt(light.readValue(), 6, 7, 5);
      LCD.drawString(nxt.getBrickName(), 0, 6);
      LCD.drawString(nxt.getFirmwareVersion(), 0, 7);
      LCD.drawString(nxt.getProtocolVersion(), 4, 7);
      LCD.drawInt(nxt.getFlashMemory(), 6, 8, 7);

      // Do we have a button press?
      int key = Button.readButtons();
      if (key != 0) {
        // New command, work out what to do.
        if (key == 1) { // ENTER
          power += 20;
          if (power > 100) power = 0;
        } else if (key == 2) { // LEFT
          mode++;
          if (mode > 4) mode = 1;
        } else if (key == 4) { // RIGHT
          motor++;
          if (motor > 2) motor = 0;
        } else if (key == 8) { // ESCAPE
          LCD.clear();
          LCD.drawString("Closing...", 0, 0);
          for (int i = 0; i < motors.length; i++) motors[i].flt();
          nxt.close();
          Thread.sleep(2000);
          System.exit(0);
        }

        LCD.clear();
        LCD.drawString("Setting power", 0, 0);
        motors[motor].setPower(power);
        LCD.drawString("Moving motor", 0, 1);
        if (mode == 1) motors[motor].forward();
        else if (mode == 2) motors[motor].backward();
        else if (mode == 3) motors[motor].flt();
        else if (mode == 4) motors[motor].stop();
        // Wait for the button to be released...
        while (Button.readButtons() != 0) Thread.yield();
        LCD.clear();
      }
    }
  }
Example #8
0
 public void action() {
   LCD.clear();
   LCD.drawInt(sonar.getDistance(), 0, 0);
   LCD.refresh();
 }