コード例 #1
0
  public static void main(String[] args) {

    UltrasonicPoller frontPoller = new UltrasonicPoller("S4");
    UltrasonicPoller sidePoller = new UltrasonicPoller("S1");
    ColorSensorPoller blockPoller = new ColorSensorPoller("S2");
    ColorSensorPoller groundPoller = new ColorSensorPoller("S3");
    groundPoller.setMode(1);
    // start the block detector thread, which will be constantly checking with the light sensor
    // to see if there is a block.
    boolean wifiWorked = true;

    // *********************************WiFi module************************************//
    // Set up WiFi connection, require data from server, parse data and disconnect from server.
    WifiConnection conn = null;
    Transmission t = null;
    try {
      conn = new WifiConnection(SERVER_IP, TEAM_NUMBER);
    } catch (IOException e) {
      LCD.drawString("Connection failed", 0, 1);
      wifiWorked = false;
    }
    if (conn == null) {
      LCD.drawString("Unable to find Server", 0, 5);
      wifiWorked = false;
    } else {
      // Data received from the server is saved in "t".
      // Pass the data saved in t to the relevant class
      t = conn.getTransmission();
      // Display the data in t
      if (t == null) {
        LCD.drawString("Failed to read transmission", 0, 5);
        wifiWorked = false;
      } else {
        conn.printTransmission();
      }
      // Button.waitForAnyPress();
      LCD.clear();
    }
    // ********************************WiFi module ends******************************//

    if (wifiWorked) {
      // variables from WIFI
      int flagType = t.flagType;
      StartCorner startingCorner = t.startingCorner;
      int bottomLeftX = t.opponentHomeZoneBL_X;
      int bottomLeftY = t.opponentHomeZoneBL_Y;
      int topRightX = t.opponentHomeZoneTR_X;
      int topRightY = t.opponentHomeZoneTR_Y;
      int capturePointX = t.dropZone_X;
      int capturePointY = t.dropZone_Y;
      // variables HARDCODED
      //			int flagType = 1;
      //			StartCorner startingCorner = StartCorner.TOP_RIGHT;
      //			int bottomLeftX = 3;
      //			int bottomLeftY = 3;
      //			int topRightX = 5;
      //			int topRightY = 5;
      //			int capturePointX = 1;
      //			int capturePointY = 1;

      // ***********************Initialization Module******************************//
      // setup the odometer
      Odometer odo = new Odometer(leftMotor, rightMotor, 30, true);
      OdometerCorrector odoCorr = new OdometerCorrector(odo, groundPoller);

      // setup the wall avoider
      WallAvoider avoider = new WallAvoider(odo, frontPoller, sidePoller);

      // set up the navigator
      Navigation navi = new Navigation(odo, avoider, frontPoller, WHEEL_RADIUS, TRACK);

      // set up the detector
      BlockDetector blockDetector =
          new BlockDetector(
              blockPoller, navi, odo, frontPoller, verticalArmMotor, horizontalArmMotor, flagType);
      blockDetector.start();
      // set up the searcher
      BlockZoneSearcher flagSearcher =
          new BlockZoneSearcher(sidePoller, frontPoller, navi, odo, blockDetector);
      // set up the LCD
      LCDDisplay lcd = new LCDDisplay(odo, frontPoller, sidePoller, blockPoller, blockDetector);
      lcd.start();

      // set up the localization
      LightLocalizer lsl =
          new LightLocalizer(
              odo, groundPoller, navi, ROBOT_CENTRE_TO_LIGHTLOCALIZATION_SENSOR, startingCorner);
      USLocalizer usl =
          new USLocalizer(odo, navi, frontPoller, USLocalizer.LocalizationType.FULL_CIRCLE);
      // ***********************End of Initialization******************************//

      double angleForSearch;
      String searchDirection = "";
      int searchStartX;
      int searchStartY;
      int firstCornerX;
      int firstCornerY;
      double zoneBuffer;
      // we use our starting corner to determine where we want to travel to in order to search.
      if (startingCorner == StartCorner.BOTTOM_LEFT || startingCorner == StartCorner.BOTTOM_RIGHT) {
        searchDirection = "down";
        searchStartX = bottomLeftX;
        searchStartY = topRightY;
        firstCornerX = bottomLeftX;
        firstCornerY = bottomLeftY;
        zoneBuffer = -1 * TILE_WIDTH / 3.0;
        angleForSearch = 270;

      } else { // it is the top left or top right
        searchDirection = "up";
        searchStartX = topRightX;
        searchStartY = bottomLeftY;
        firstCornerX = topRightX;
        firstCornerY = topRightY;
        zoneBuffer = TILE_WIDTH / 3.0;
        angleForSearch = 90;
      }

      /*
       * Step 1: Ultrasonic Localization
       * 	Figure out where we are, roughly
       */
      // disable the side sensor for localization so that it doesn't interfere
      sidePoller.disableSensor();
      // set the errors on the navigation to be large for US localization
      navi.setCmError(0.5);
      navi.setDegreeError(4.0);
      // perform ultra-sonic localization
      usl.doLocalization();

      /*
       * Step 2: Light Localization
       * 	Figure out where we are, precisely
       */
      // set the errors back to smaller values
      navi.setCmError(0.5);
      navi.setDegreeError(2.0);
      // perform light-sensor localization
      lsl.doLocalization();

      /*
       * Step 3: Travel to first corner
       * 	Travel to the corner of the block zone in which we are going to relocalize
       */
      // enable the side poller for navigating
      sidePoller.enableSensor();
      // speed up the robot for navigation
      navi.setSlowSpeed(90);
      navi.setFastSpeed(160);
      // start odometry correction
      odoCorr.start();
      // navigation
      navi.travelToAndAvoid(TILE_WIDTH * firstCornerX, TILE_WIDTH * firstCornerY);

      /*
       * Step 4: Relocalize
       * 	Use the lines to relocalize (figure out where we are, again)
       */
      // perform second localization
      lsl.doRelocalization(TILE_WIDTH * firstCornerX, TILE_WIDTH * firstCornerY);

      /*
       * Step 5: Travel to second corner & rotate
       * 	Travel to the top (or bottom) corner of the block zone, and turn to searching position
       */
      // we first travel a bit away from the zone
      navi.travelTo(TILE_WIDTH * firstCornerX + zoneBuffer, TILE_WIDTH * firstCornerY + zoneBuffer);
      // we travel to the second corner with no avoidance (there can't be any blocks there, anyway)
      navi.travelTo(TILE_WIDTH * searchStartX + zoneBuffer, TILE_WIDTH * searchStartY);
      navi.turnTo(angleForSearch, true);

      /*
       * Step 6: Search for block
       * 	Start to search around the perimeter for the block
       */
      // lower errors
      navi.setCmError(0.4);
      navi.setDegreeError(3);
      // find dimensions of place to search
      int zoneWidth = topRightX - bottomLeftX;
      int zoneHeight = topRightY - bottomLeftY;
      // run searcher
      flagSearcher.searchZone(searchStartX, searchStartY, zoneWidth, zoneHeight, searchDirection);

      /*
       * Step 7: Drive to end position
       * 	We now have the block. Drive to the final position
       */
      if (blockDetector.isFlag()) {
        // we want to travel to the center of the blocks.
        navi.travelToAndAvoid(
            capturePointX * TILE_WIDTH + TILE_WIDTH / 2,
            capturePointY * TILE_WIDTH + TILE_WIDTH / 2); // wifi
      }

      while (Button.waitForAnyPress() != Button.ID_ESCAPE) ;
      System.exit(0);
    }
  }
コード例 #2
0
ファイル: Lab5.java プロジェクト: sebastien177/DPM
  public static void main(String[] args) {
    int buttonChoice;

    // setup the odometer and display
    Odometer odo = new Odometer(leftMotor, rightMotor, 30, true);
    final TextLCD t = LocalEV3.get().getTextLCD();

    do {
      // clear the display
      t.clear();

      // ask the user whether he wants to detect blocks or search blocks
      t.drawString("< Left  |Right >", 0, 0);
      t.drawString("        |       ", 0, 1);
      t.drawString("Detect|Search ", 0, 2);

      buttonChoice = Button.waitForAnyPress();

      while (buttonChoice != Button.ID_LEFT
          && buttonChoice != Button.ID_RIGHT
          && buttonChoice != Button.ID_ESCAPE) {
        /*
         * These two if statements is to make the motor attached to the USsensor rotate
         * 90 degrees before the main methods are launched
         */
        if (buttonChoice == Button.ID_UP) {
          Scan scan = new Scan(usMotor);
          scan.usMotorSpeed(50);
          scan.turnSensor(90);
          buttonChoice = Button.waitForAnyPress();
        }

        if (buttonChoice == Button.ID_DOWN) {
          Scan scan = new Scan(usMotor);
          scan.usMotorSpeed(50);
          scan.turnSensor(-90);
          buttonChoice = Button.waitForAnyPress();
        }
      }
    } while (buttonChoice != Button.ID_LEFT
        && buttonChoice != Button.ID_RIGHT
        && buttonChoice != Button.ID_ESCAPE);

    if (buttonChoice == Button.ID_ESCAPE) {
      System.exit(0);
    }

    SensorModes usSensor = new EV3UltrasonicSensor(usPort);
    SampleProvider usValue =
        usSensor.getMode("Distance"); // colorValue provides samples from this instance
    float[] usData =
        new float[usValue.sampleSize()]; // colorData is the buffer in which data are returned

    SensorModes colorSensor = new EV3ColorSensor(colorPort);
    SampleProvider colorValue =
        colorSensor.getMode("ColorID"); // colorValue provides samples from this instance
    float[] colorData =
        new float[colorValue.sampleSize()]; // colorData is the buffer in which data are returned

    // The following start the PartA of the Lab when the right button is pressed, afterwards press
    // escape to exit program
    while (buttonChoice != Button.ID_RIGHT && buttonChoice != Button.ID_ESCAPE) {
      if (buttonChoice == Button.ID_LEFT) {
        ObjectDetection od = new ObjectDetection(colorValue, colorData, usValue, usData);
        od.run();
        LCD.drawString("< Left  |Right >", 0, 0);
        LCD.drawString("        |       ", 0, 1);
        LCD.drawString("Detect|Search ", 0, 2);
      }
      buttonChoice = Button.waitForAnyPress();
    }

    if (buttonChoice == Button.ID_ESCAPE) {
      System.exit(0);
    }
    // If the left button is pressed, the robot will start partB of the lab which is localize, and
    // start scanning the field

    odo.start();
    final USLocalizer usl =
        new USLocalizer(
            odo,
            usSensor,
            usData,
            USLocalizer.LocalizationType.FALLING_EDGE,
            leftMotor,
            rightMotor,
            WHEEL_RADIUS,
            TRACK);
    final Scan scan =
        new Scan(usValue, usData, colorValue, colorData, odo, leftMotor, rightMotor, usMotor);
    br = new BlockRecognition(odo, usSensor, usData, colorValue, colorData, rightMotor, leftMotor);

    new LCDInfo(odo, usSensor, usData, colorSensor, colorData);

    // begin the threads (we launch a thread to be able to exit it whenever we want using escape)
    (new Thread() {
          public void run() {
            br.start();
            scan.start();
            usl.doLocalization();
            scan.startRun();
          }
        })
        .start();
    while (Button.waitForAnyPress() != Button.ID_ESCAPE) ;
    System.exit(0);
  }