Пример #1
0
 public void taskSetUp(int task) {
   LCD.clear();
   if (menuSelection == 1) {
     linePID = new PID(1, 0, 0); // Initialized
     rangeFinderPID = new PID(1, 0, 0);
     LCD.drawString("Task 1-2", 0, 0);
     setPower = 80;
     echoTarget = 200;
   } else if (menuSelection == 2) {
     double a = 1.7;
     double b = 1.6;
     linePID = new PID(0.33 * a, 2 * a / b, a * b / 3);
     rangeFinderPID = new PID(4, 6, .1);
     LCD.drawString("Task 3", 0, 0);
     setPower = 20;
     echoTarget = 890;
     //			motors.enableReverse();
   } else {
     LCD.drawString("Invalid Task", 0, 0);
     LCD.drawString("Left for", 0, 2);
     LCD.drawString("Task 1-2 'curve'", 0, 3);
     LCD.drawString("Right for", 0, 5);
     LCD.drawString("Task 3 'platoon'", 0, 6);
     menuSelection = getTaskNumber();
     taskSetUp(menuSelection);
   }
   LCD.drawString("Press Enter", 0, 1);
   LCD.drawString("to Start", 0, 2);
 }
Пример #2
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;
  }
  static void init() throws InterruptedException {
    Sound.beep();
    try {
      File f = new File("CalibrationData.dat");

      FileInputStream is = new FileInputStream(f);
      colors = RescueColors.readObject(is);
      is.close();
      LCD.drawString("File read", 0, 0);
      LCD.drawString("sucessfully", 0, 0);
    } catch (IOException e) {
      LCD.drawString("Error reading", 0, 0);
      LCD.drawString("file", 0, 1);
      Thread.sleep(1000);
      System.exit(-1);
    }
    colors.printToLCD();
    robot = new RescueRobot(colors);
    Button.ENTER.waitForPressAndRelease();

    logger = new MovementLogger(robot);
    Thread t = new Thread(logger);
    t.setDaemon(true);
    t.start();
  }
Пример #4
0
  /**
   * Gives us the choice between setPower(), or setSpeed(). It allowed us to test which one we
   * should use.
   */
  public static void test() {

    /*
     * First, get the values from the user by displaying the instructions on
     * the screen. If the user agrees with the parameters set in the menu,
     * proceed to the launching. If not, display the menu again.
     */

    while (!mainMenu.summarizeLauncher()) {

      if (mainMenu.display()) {
        launcherPower = mainMenu.askForPower();
      } else {
        launcherSpeed = mainMenu.askForSpeed();
      }

      launcherDelay = mainMenu.askForDelay();
    }

    LCD.clearDisplay();
    LCD.drawString("     Shooting     ", 0, 0);

    if (launcherPower != -1) {

      launcher.chainFireAtPower(launcherPower, launcherDelay);

    } else {

      launcher.chainFireAtSpeed(launcherSpeed, launcherDelay);
    }

    LCD.clearDisplay();
  }
Пример #5
0
  public static void main(String[] args) throws InterruptedException {
    // Creates an area of motors
    Motor[] m = {Motor.A, Motor.B};

    // Spins them forward for 3 seconds
    for (int i = 0; i < 2; i++) {
      m[i].forward();
    }
    LCD.drawString("FORWARD", 0, 0);
    Thread.sleep(3000);

    // Spins them backward for 3 seconds
    for (int i = 0; i < 2; i++) {
      m[i].backward();
    }
    LCD.drawString("BACKWARD", 0, 1);
    Thread.sleep(3000);

    // Spins them forward again for 3 seconds
    for (int i = 0; i < 2; i++) {
      m[i].reverseDirection();
    }
    LCD.drawString("FORWARD", 0, 2);
    Thread.sleep(3000);

    // Stops the motors
    for (int i = 0; i < 2; i++) {
      m[i].stop();
    }
  }
Пример #6
0
 public synchronized void run() {
   if ((line >= 0) && (line <= 7)) {
     LCD.drawString("                ", 0, getLine());
     LCD.drawString(getAString(), 0, getLine());
     LCD.refresh();
   }
 }
Пример #7
0
  public static void main(String[] args) {
    // initialiseer alle equipment
    tempMotor = new RCXMotor(MotorPort.A);
    lamp = Motor.C;
    colorsens = new ColorSensor(SensorPort.S1);
    sonar = new UltrasonicSensor(SensorPort.S2);
    tempSensor = new RCXTemperatureSensor(SensorPort.S3);
    lakes = new Lake[3];
    lakes[0] = new Lake(Color.RED);
    lakes[1] = new Lake(Color.BLUE);
    lakes[2] = new Lake(Color.GREEN);

    // zet de robot in de beginstate
    current = State.BTINIT;

    // opstart-info
    LCD.drawString("EndGameRobot", 0, 1);
    LCD.drawString("Judith & Mirjam", 0, 2);
    Button.waitForAnyPress();

    // start de loop of doom
    while (!inEndState()) {
      execute(current);
    }
    execute(current); // execute Endstate
  }
Пример #8
0
  public static void main(String args[]) throws InterruptedException {
    LCD.drawString("Hello World!", 1, 1);
    LCD.drawString("Second line", 1, 2);
    LCD.drawString("Third line", 1, 3);

    Button.ESCAPE.waitForPressAndRelease();
  }
Пример #9
0
 public static void main(String[] args) throws Exception {
   RCXTemperatureSensor temperature = new RCXTemperatureSensor(SensorPort.S1);
   boolean s = true;
   while (!Button.ESCAPE.isDown()) {
     LCD.clear();
     if (s) LCD.drawString("C " + temperature.getCelcius(), 0, 0);
     else LCD.drawString("F " + temperature.getFahrenheit(), 0, 0);
     s = !s;
     LCD.refresh();
     Thread.sleep(2000);
   }
 }
Пример #10
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));
    }
  }
Пример #11
0
  public static void Measurement() {
    // execute all actions of this state
    LCD.drawString("Measurement", 0, 3);
    btThread.popElement();
    Sound.beep();
    Sound.beep();
    // lower sensor by RCX motor
    tempMotor.setPower(-100);
    tempMotor.setPower(0);
    for (int i = 0; i < nrcolors; i++) {
      if (colorsens.getColorID() == lakes[i].color && !lakes[i].found) // kleuren komen overeen
      lakes[i].celsius = tempSensor.getCelcius();
      lakes[i].found = true;
      return;
    }
    // raise temp sensor
    tempMotor.setPower(100);
    btThread.write(500); // sends the message 'action done'

    // leg de huidige tijd vast voor alle transitions met een timeoutcondition
    long starttime = System.currentTimeMillis();

    // when done, wait for a trigger for a transition
    boolean transitionTaken = false;
    while (!transitionTaken) {
      if ((lakes[0].found && lakes[1].found && lakes[2].found)) {
        current = State.FINISHED;
        transitionTaken = true;
      } else if ((true)) {
        current = State.WATCH;
        transitionTaken = true;
      }
    }
  }
Пример #12
0
 public void run() {
   while (true) {
     boolean kick = ControlCentre.getKickState();
     if (kick) {
       LCD.drawString("K,", 0, 1);
       Movement.motor_kick.setSpeed(900);
       Movement.motor_kick.rotate((-120 * (5 / 3)));
       Movement.motor_kick.rotate((120 * (5 / 3)));
     } else {
       LCD.drawString("_,", 0, 1);
     }
     try {
       Thread.sleep(100);
     } catch (InterruptedException e) {
     }
   }
 }
Пример #13
0
  public static void main(String[] args) throws Exception {
    String[] connectionStrings = new String[] {"Bluetooth", "USB", "RS485"};
    TextMenu connectionMenu = new TextMenu(connectionStrings, 0, "Connection");
    NXTCommConnector[] connectors = {
      Bluetooth.getConnector(), USB.getConnector(), RS485.getConnector()
    };

    int connectionType = connectionMenu.select();
    LCD.clear();
    LCD.clear();
    LCD.drawString("Type: " + connectionStrings[connectionType], 0, 0);
    LCD.drawString("Running...", 0, 1);
    Responder resp = new Responder(connectors[connectionType]);
    resp.start();
    resp.join();
    LCD.drawString("Closing...  ", 0, 1);
  }
  // The recognize() method returns true if the object is a blue block
  public boolean recognize() {

    // Boolean that determines whether or not to start detecting what kind of object is in front
    boolean isRecognizing = true;

    while (isRecognizing) {

      // The robot will drive slowly if the light reads under 300, which means an object is closeby
      while (color.getNormalizedLightValue() < 300) {
        robot.setForwardSpeed(2);
      }

      // When the robot is close enough to the object, it will stop then turn on its
      // blue light which gives two noticeable different light values for a wooden and blue
      // block

      robot.setForwardSpeed(0);
      color.setFloodlight(Color.BLUE);

      // If the light value of blue reads over 250, it is a blue block
      if (color.getNormalizedLightValue() >= 250) {

        Sound.beep();
        LCD.drawString("Blue styrofoam block", 3, 5);

        // Turn the default light back on after analyzing an object
        color.setFloodlight(true);

        return true;

      } else { // If it is not a blue block, it is a wooden block

        Sound.buzz();
        LCD.drawString("Wooden block", 3, 5);

        // Turn the default light back on after analyzing an object
        color.setFloodlight(true);
      }

      // Set the boolean to false to break out of the while loop and stop analyzing the object
      isRecognizing = false;
    }

    // Default return value. It should never get here but JAVA requires a return type
    return false;
  }
Пример #15
0
 public static void Finished() {
   // execute all actions of this state
   LCD.drawString("Finished", 0, 3);
   Sound.beep();
   Sound.beep();
   Sound.beep();
   btThread.write(300); // sends the message 'all done'
 }
  public static void main(String[] args) {
    LCD.drawString(appName, 0, 0);
    LCD.drawString("#################", 0, 2);
    LCD.drawString("#################", 0, 6);

    msc = new MSC(SensorPort.S1);
    // Set to initial angle
    msc.servo1.setAngle(90);

    int angle = 0;
    int pulse = 0;
    int NXTServoBattery = 0;

    while (!Button.ESCAPE.isPressed()) {
      NXTServoBattery = msc.getBattery();

      if (Button.LEFT.isPressed()) {
        angle = 0;
        msc.servo1.setAngle(angle);
      }

      if (Button.ENTER.isPressed()) {
        angle = 90;
        msc.servo1.setAngle(angle);
      }

      if (Button.RIGHT.isPressed()) {
        angle = 180;
        msc.servo1.setAngle(angle);
      }

      clearRows();
      LCD.drawString("Battery: " + NXTServoBattery, 0, 3);
      LCD.drawString("Pulse:   " + msc.servo1.getPulse(), 0, 4);
      LCD.drawString("Angle:   " + msc.servo1.getAngle(), 0, 5);
      LCD.refresh();
    }

    // Set to initial angle
    msc.servo1.setAngle(90);

    LCD.drawString("Test finished", 0, 7);
    LCD.refresh();
    try {
      Thread.sleep(1000);
    } catch (Exception e) {
    }
    credits(3);
    System.exit(0);
  }
 /** Shows the calibration parameters, ie offset and scale */
 public void showCalibrationSettings() {
   LCD.clear();
   LCD.drawString("offset range", 3, 0);
   LCD.drawString("X", 0, 2);
   LCD.drawString("Y", 0, 3);
   LCD.drawString("Z", 0, 4);
   LCD.drawString("Escape to return", 0, 7);
   for (int i = 0; i < 3; i++) {
     LCD.drawString(Double.toString(offset[i]), 3, i + 2);
     LCD.drawString(" ", 11, i + 2);
     LCD.drawString(Double.toString(scale[i]), 10, i + 2);
   }
   Button.ESCAPE.waitForPressAndRelease();
 }
Пример #18
0
  /**
   * Asks the user what speed to use for every shoots. This method is used as testing for the
   * launcher.
   */
  public static void testSpeed() {

    launcherDelay = mainMenu.askForDelay();
    launcherSpeed = mainMenu.askForSpeed(700);
    // Loop if nobody presses the escape button
    while (iButton != Button.ID_ESCAPE) {

      LCD.clear();
      LCD.drawString("S:" + launcherSpeed, 0, 1);
      LCD.drawString("D:" + launcherDelay, 0, 2);
      LCD.drawString("V:" + Battery.getVoltageMilliVolt(), 0, 3);

      launcher.chainFireAtSpeed(launcherSpeed, launcherDelay);

      launcherSpeed = mainMenu.askForSpeed(launcherSpeed);
      iButton = Button.waitForPress();
    }
  }
Пример #19
0
  public Follow() {
    // TODO Auto-generated constructor stub
    ultrasonicSensor = new UltrasonicSensorEcho(49, SensorPort.S4);
    display = new FollowDisplay();
    LCD.drawString("Left for", 0, 0);
    LCD.drawString("Task 1-2 'curve'", 0, 1);
    LCD.drawString("Right for", 0, 3);
    LCD.drawString("Task 3 'platoon'", 0, 4);
    IRArray = new IRSensorArray(SensorPort.S1, SensorPort.S2, SensorPort.S3);
    menuSelection = getTaskNumber();
    taskSetUp(menuSelection);
    motors = new MotorController(MotorPort.A, MotorPort.B);
    SensorPort.S4.setSensorPinMode(SensorPort.SP_DIGI0, SensorPort.SP_MODE_INPUT);
    SensorPort.S4.setSensorPinMode(SensorPort.SP_DIGI1, SensorPort.SP_MODE_OUTPUT);
    display.follow = this;

    new Thread(this).start();
  }
Пример #20
0
 public void run() {
   int count = 0;
   while (true) {
     LCD.drawString("" + Integer.toString(count++), 6, 0);
     count %= 1000;
     try {
       Thread.sleep(100);
     } catch (InterruptedException e) {
     }
   }
 }
Пример #21
0
 public static void main(String[] arguments) {
   Motor.A.setSpeed(200);
   Motor.B.setSpeed(200);
   Motor.A.forward();
   Motor.B.forward();
   final LightSensor sensor = new LightSensor(SensorPort.S1);
   final Object blocker = new Object();
   while (true) {
     if (sensor.readNormalizedValue() >= 512) {
       LCD.bitBlt(
           null,
           LCD.SCREEN_WIDTH,
           LCD.SCREEN_HEIGHT,
           0,
           0,
           0,
           0,
           LCD.SCREEN_WIDTH,
           LCD.SCREEN_HEIGHT,
           LCD.ROP_CLEAR);
     } else {
       LCD.bitBlt(
           null,
           LCD.SCREEN_WIDTH,
           LCD.SCREEN_HEIGHT,
           0,
           0,
           0,
           0,
           LCD.SCREEN_WIDTH,
           LCD.SCREEN_HEIGHT,
           LCD.ROP_SET);
     }
     synchronized (blocker) {
       try {
         blocker.wait(100);
       } catch (InterruptedException e) {
       }
     }
   }
 }
Пример #22
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();
  }
  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();
  }
Пример #24
0
  // a method for every state
  public static void Btinit() {
    // execute all actions of this state
    LCD.drawString("Btinit", 0, 3);
    LCD.drawString("Waiting...", 0, 0);
    LCD.refresh();

    BTConnection btc = Bluetooth.waitForConnection();

    LCD.clear();
    LCD.drawString("Connected", 0, 0);
    LCD.refresh();

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

    // BTfunctionality btThread = new BTfunctionality("SlaveReader",dis,dos);
    btThread = new BTfunctionality("SlaveReader", dis, dos);
    btThread.start();

    // leg de huidige tijd vast voor alle transitions met een timeoutcondition
    long starttime = System.currentTimeMillis();

    // when done, wait for a trigger for a transition
    boolean transitionTaken = false;
    while (!transitionTaken) {
      if ((true)) {
        current = State.WATCH;
        transitionTaken = true;
      }
    }
  }
 /**
  * Displays the raw value of axis indicated by index. A low pass filter is applied on the raw
  * values to make reading easier.
  *
  * @param index Indicates the axis (x=0,y=1,z=2).
  */
 private void showLowPass(int index) {
   float alpha = 0.95f;
   float lowpassed = 0;
   while (!Button.ENTER.isDown()) {
     accel.fetchRawAccel(raw);
     lowpassed = alpha * raw[index] + (1.0f - alpha) * lowpassed;
     LCD.drawString(Double.toString(lowpassed), 0, 3);
     try {
       Thread.sleep(150);
     } catch (InterruptedException ex) {
     }
   }
   while (Button.ENTER.isDown()) ;
 }
Пример #26
0
  public void run() {

    try {

      // TODO: clean closing of connection

      for (; ; ) {

        BTConnection btc = Bluetooth.waitForConnection();
        DataInputStream dis = btc.openDataInputStream();

        // Read Input-Number from BTSend
        int n = dis.readInt();

        switch (n) {
          case 0:
            // up
            swp.forward();
            break;
          case 1:
            // down
            swp.backward();
            break;
          case 2:
            // left
            swp.arc(-1, 60, true);
            break;
          case 3:
            // right
            swp.arc(1, 60, true);
            break;
          case 4:
            // Stop
            swp.stop();
            break;
          default:
            // do nothing
            break;
        }

        LCD.clear();
        dis.close();
        btc.close();
      }

    } catch (Exception ioe) {
      // LCD.drawString(ioe.getMessage(), 2, 0);
    }
  }
 /**
  * Final Message
  *
  * @param seconds
  */
 private static void credits(int seconds) {
   LCD.clear();
   LCD.drawString("LEGO Mindstorms", 0, 1);
   LCD.drawString("NXT Robots  ", 0, 2);
   LCD.drawString("run better with", 0, 3);
   LCD.drawString("Java leJOS", 0, 4);
   LCD.drawString("www.lejos.org", 0, 6);
   LCD.refresh();
   try {
     Thread.sleep(seconds * 1000);
   } catch (Exception e) {
   }
 }
Пример #28
0
 @Override
 public void run() {
   while (ca.isKeepOn()) {
     if (!cmdList.isEmpty()) {
       Command cmd = removeCommand();
       cmd.execute();
     }
     try {
       Thread.sleep(100);
     } catch (InterruptedException e) {
       e.printStackTrace();
     }
   }
   LCD.drawString("exit CommandExecutor", 0, 0);
 }
Пример #29
0
  public static void Sonarflag() {
    // execute all actions of this state
    LCD.drawString("Sonarflag", 0, 3);
    Sound.beep();
    btThread.write(sonar.getDistance()); // sends the data from the sonar

    // leg de huidige tijd vast voor alle transitions met een timeoutcondition
    long starttime = System.currentTimeMillis();

    // when done, wait for a trigger for a transition
    boolean transitionTaken = false;
    while (!transitionTaken) {
      if ((true)) {
        current = State.WATCH;
        transitionTaken = true;
      }
    }
  }
  public static void main(String[] args) {
    HTSensorMux3 sm1 = new HTSensorMux3(SensorPort.S1);

    // LCD.drawString("" + sm1.getProductID(), 0,2);
    // LCD.drawString("" + sm1.getVersion(), 0,3);
    // LCD.drawString("" + sm1.getSensorType(), 0,4);
    // LCD.refresh();

    sm1.configurateMUX2();

    int distance = 0;
    while (!Button.ESCAPE.isPressed()) {
      distance = sm1.getDistance();
      LCD.drawString("" + distance, 0, 0);
      try {
        Thread.sleep(100);
      } catch (Exception e) {
      }
    }
  }