コード例 #1
0
 public void checkIfSharpCorner() {
   leftMotor.stop();
   rightMotor.stop();
   isSharpCorner = false;
   int performedRotation = 0;
   DifferentialPilot pilot = new DifferentialPilot(2, 10, leftMotor, rightMotor);
   if (getRealTimeValue() >= 0.1) {
     pilot.rotate(-4);
     isSharpCorner = true;
     tellLineBehaviorIsSharpCorner();
   } else {
     for (int i = 0; i <= 40; i++) {
       pilot.rotate(1);
       performedRotation += 1;
       if (getRealTimeValue() >= 0.1) {
         isSharpCorner = true;
         tellLineBehaviorIsSharpCorner();
         break;
       }
       if (Button.readButtons() != 0) {
         this.interrupt();
       }
     }
   }
   if (!isSharpCorner) {
     align(pilot, performedRotation);
     line.setIsNoSharpCorner();
   }
 }
コード例 #2
0
  public void init() {
    Button.LEDPattern(2);
    colorSensor = new EV3ColorSensor(s1);
    colorSensor.setFloodlight(Color.WHITE);
    log = new ArrayList<>();
    previousLogging = System.currentTimeMillis();
    logger.info("Initialized");

    Runnable r =
        new Runnable() {
          public void run() {
            while (!stopped) {
              int detectedColorId = colorSensor.getColorID();

              long now = System.currentTimeMillis();
              log.add(String.valueOf(now - previousLogging));
              previousLogging = now;

              if (detectedColorId == lastRegisteredColorId) {
                sleep();
                continue;
              }

              if (detectedColorId != lastDetectedColorId) {
                lastDetectedColorId = detectedColorId;
                readings = 1;
                sleep();
                continue;
              }

              readings++;
              if (detectedColorId < HIGHEST_SIGNAL_COLOR_ID
                  || readings == CONSECUTIVE_READINGS_REQUIREMENT) {
                lastRegisteredColorId = detectedColorId;
                if (detectedColorId != BACKGROUND_COLOR_ID) {
                  sendToBlock("SLEEPER", SleeperColor.convertFromLejosColor(detectedColorId));
                }
              }

              sleep();
            }
          }
        };

    runAsync(r);
  }
コード例 #3
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);
    }
  }
コード例 #4
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);
  }
コード例 #5
0
ファイル: Driver.java プロジェクト: soCzech/robo-okami
  public void run() {

    while (true) {
      if (_suppress) {
        Thread.yield();
        continue;
      }
      Pose oldPose = cachePose;
      cachePose = Rover.poseProvider.getPose();

      // get angle and distance where to travel
      int[] way = wayClass.getPredefinedWay(oldPose, cachePose);
      cacheWayAngle = way[1];
      cacheWayDistance = way[0] * Const.BLOCK_WIDTH;

      float angle = Const.normalizeAngle(cacheWayAngle - cachePose.getHeading());

      if (Const.angleToByte(angle) == 2 && cacheWayDistance == Const.BLOCK_WIDTH) {
        // if the robot is supposed to rotate 180 deg and travel one block, back out
        Rover.pilot.travel(-cacheWayDistance, true);
      } else {
        // otherwise rotate and travel the distance
        if (Math.abs(angle) > 2) {
          // float cacheGyro = Rover.getGyro();
          boolean touched = false;

          Rover.pilot.rotate(angle, true);

          while (Rover.pilot.isMoving()) {
            boolean[] touch = Rover.getTouch();
            if (touch[0] || touch[1]) touched = true;
            Thread.yield();
          }
          Pose newPose = Rover.poseProvider.getPose();

          /*
          float angleGyro = Rover.getGyro() - cacheGyro;

          // if the turn is incomplete, rotate by the difference
          // TODO maybe turn only when the difference is bigger
          if (angle > angleGyro + 5 || angle < angleGyro - 5) {
          	Rover.pilot.travel(touched ? -5 : 5);
          	cacheWayDistance += touched ? 5 : -5;
          	Rover.pilot.rotate(angle - angleGyro);

          	newPose = Rover.poseProvider.getPose();
          } */

          // update pose with normalize heading
          Rover.poseProvider.setPose(new Pose(newPose.getX(), newPose.getY(), cacheWayAngle));
        }

        float blockAngle = (float) Math.toRadians(cacheWayAngle);
        byte sin = (byte) Math.round(Math.sin(blockAngle)),
            cos = (byte) Math.round(Math.cos(blockAngle));
        byte
            blockX =
                (byte)
                    (Const.roundToMapArea(Const.middlePointToBlock(cachePose.getX()), "X")
                        + cos * (way[0] + 1)),
            blockY =
                (byte)
                    (Const.roundToMapArea(Const.middlePointToBlock(cachePose.getY()), "Y")
                        + sin * (way[0] + 1));

        // extend the distance to hit a wall
        if (blockX < 0
            || blockY < 0
            || blockX > 8
            || blockY > 5
            || wayClass.getOccupancyMap(blockX, blockY) > 0) {

          cacheWayDistance += Const.BLOCK_WIDTH;
          // TODO no way of updating pose if it doesn't hit wall
        }
        Rover.pilot.travel(cacheWayDistance, true);
      }

      // while the rover is traveling
      while (Rover.pilot.isMoving()) {
        boolean[] touch = Rover.getTouch();
        if (touch[0] || touch[1]) {
          Rover.pilot.quickStop();
          float sonic = Rover.getSonic();

          Pose hitPose = Rover.poseProvider.getPose();

          if (hitTheWall(hitPose)) {
            Rover.pilot.travel(-4);
          } else if (sonic > 15 && !(touch[0] && touch[1])) {
            // if the rover touches the obstacle by just one sensor
            // and distance is more than 15 cm
            float distanceTraveled = Rover.pilot.getMovement().getDistanceTraveled();
            Rover.pilot.travel(-4);
            Rover.pilot.rotate(touch[0] ? -20 : 20);

            Pose newPose = Rover.poseProvider.getPose();
            Rover.poseProvider.setPose(new Pose(newPose.getX(), newPose.getY(), cacheWayAngle));

            cacheWayDistance -= distanceTraveled - 4;
            Rover.pilot.travel(cacheWayDistance, true);

          } else {
            // if the obstacle touched by both sensors
            // or the distance is less than 15 cm
            Pose p = Rover.poseProvider.getPose();

            byte blockX = Const.roundToMapArea(Const.middlePointToBlock(p.getX()), "X"),
                blockY = Const.roundToMapArea(Const.middlePointToBlock(p.getY()), "Y");
            float blockAngle = (float) Math.toRadians(Const.angleToByte(p.getHeading()) * 90);
            byte sin = (byte) Math.round(Math.sin(blockAngle)),
                cos = (byte) Math.round(Math.cos(blockAngle));

            boolean isFree;
            try {
              isFree = wayClass.getOccupancyMap((byte) (blockX + cos), (byte) (blockY + sin)) < 0;
            } catch (ArrayIndexOutOfBoundsException e) {
              isFree = false;
            }

            if (!(touch[0] && touch[1]) && isFree) {
              Rover.pilot.rotate(touch[0] ? -90 : 90);
              Rover.pilot.travel(15);
              Rover.pilot.rotate(touch[0] ? 90 : -90);

              Rover.poseProvider.setPose(
                  new Pose(
                      (blockX + 1 / 2) * Const.BLOCK_WIDTH,
                      (blockY + 1 / 2) * Const.BLOCK_WIDTH,
                      Const.angleToByte(p.getHeading()) * 90));

              float distanceTraveled = Rover.pilot.getMovement().getDistanceTraveled();
              cacheWayDistance -= distanceTraveled;

              Rover.pilot.travel(cacheWayDistance, true);
            } else {
              System.out.println("ERROR: TRAVEL CORRECTION");
              // This shouldn't happen any more
              Button.LEDPattern(5);
            }
          }
        }
        Thread.yield();
      }
    }
  }