Пример #1
0
public class OdometerDisplay implements TimerListener {
  public static final int LCD_REFRESH = 100;
  private Odometer odo;
  private Timer lcdTimer;
  private TextLCD LCD = LocalEV3.get().getTextLCD();;

  // arrays for displaying data
  private double[] pos;

  public OdometerDisplay(Odometer odo) {
    this.odo = odo;
    this.lcdTimer = new Timer(LCD_REFRESH, this);

    // initialise the arrays for displaying data
    pos = new double[3];

    // start the timer
    lcdTimer.start();
  }

  public void timedOut() {
    odo.getPosition(pos);
    LCD.clear();
    LCD.drawString("X: ", 0, 0);
    LCD.drawString("Y: ", 0, 1);
    LCD.drawString("H: ", 0, 2);
    LCD.drawInt((int) (pos[0]), 3, 0);
    LCD.drawInt((int) (pos[1]), 3, 1);
    LCD.drawInt((int) pos[2], 3, 2);
  }
}
Пример #2
0
 /** Download all files required for Open Roberta Lab. */
 public void update() {
   this.update_error = false;
   getRuntime();
   getShared();
   getJsonLib();
   getEV3Menu();
   if (this.update_error == false) {
     LocalEV3.get().getAudio().systemSound(Sounds.ASCENDING);
     ORAhandler.setRestarted(false);
     ORAhandler.setRegistered(false);
     ORAhandler.setConnectionError(false);
     Delay.msDelay(1000);
     LocalEV3.get().getAudio().systemSound(Sounds.DESCENDING);
   } else {
     LocalEV3.get().getAudio().systemSound(Sounds.BUZZ);
   }
 }
Пример #3
0
public class LocalizationTest {

  public static EV3LargeRegulatedMotor rightMotor =
      new EV3LargeRegulatedMotor(LocalEV3.get().getPort("D"));
  public static EV3LargeRegulatedMotor leftMotor =
      new EV3LargeRegulatedMotor(LocalEV3.get().getPort("A"));
  public static EV3MediumRegulatedMotor clawMotor =
      new EV3MediumRegulatedMotor(LocalEV3.get().getPort("C"));
  public static Odometer odo = new Odometer(leftMotor, rightMotor);
  public static Navigation nav = new Navigation(odo, leftMotor, rightMotor);
  private static UltrasonicPoller uss = new UltrasonicPoller(10, 5, 5, 50, 0);

  public static LCDdisplay lcd = new LCDdisplay(odo, uss, null, null);

  public static void main(String[] args) {
    USLocalizer localization = new USLocalizer(nav, odo, uss, lcd);
    localization.doLocalization(30);
  }
}
Пример #4
0
  public EV3Bot() {
    this.botMessage = "Lejos!";
    this.xPosition = 0;
    this.yPosition = 2;
    this.waitTime = 4000;
    this.turnWaitTime = 500;

    distanceSensor = new EV3UltrasonicSensor(LocalEV3.get().getPort("S1"));
    ultrasonicSamples = new float[distanceSensor.sampleSize()];
    distanceSensor.enable();
    setupPilot();
  }
 protected RMIRemoteRegulatedMotor(String portName, char motorType) throws RemoteException {
   super(0);
   Port p = LocalEV3.get().getPort(portName);
   switch (motorType) {
     case 'N':
       motor = new NXTRegulatedMotor(p);
       break;
     case 'L':
       motor = new EV3LargeRegulatedMotor(p);
       break;
     case 'M':
       motor = new EV3MediumRegulatedMotor(p);
       break;
     case 'G':
       motor = new MindsensorsGlideWheelMRegulatedMotor(p);
   }
 }
Пример #6
0
 public Buttoni() {
   port3 = LocalEV3.get().getPort("S2");
   tsensor = new EV3TouchSensor(port3);
 }
Пример #7
0
public class RMIRemoteGraphicsLCD extends UnicastRemoteObject implements RMIGraphicsLCD {

  private static final long serialVersionUID = 1561883712422890550L;
  private GraphicsLCD g = LocalEV3.get().getGraphicsLCD();

  protected RMIRemoteGraphicsLCD() throws RemoteException {
    super(0);
  }

  @Override
  public void setPixel(int x, int y, int color) throws RemoteException {
    g.setPixel(x, y, color);
  }

  @Override
  public int getPixel(int x, int y) throws RemoteException {
    return g.getPixel(x, y);
  }

  @Override
  public void drawString(String str, int x, int y, int anchor, boolean inverted)
      throws RemoteException {
    g.drawString(str, x, y, anchor, inverted);
  }

  @Override
  public void drawString(String str, int x, int y, int anchor) throws RemoteException {
    g.drawString(str, x, y, anchor);
  }

  @Override
  public void drawSubstring(String str, int offset, int len, int x, int y, int anchor)
      throws RemoteException {
    g.drawSubstring(str, offset, len, x, y, anchor);
  }

  @Override
  public void drawChar(char character, int x, int y, int anchor) throws RemoteException {
    g.drawChar(character, x, y, anchor);
  }

  @Override
  public void drawChars(char[] data, int offset, int length, int x, int y, int anchor)
      throws RemoteException {
    g.drawChars(data, offset, length, x, y, anchor);
  }

  @Override
  public int getStrokeStyle() throws RemoteException {
    return g.getStrokeStyle();
  }

  @Override
  public void setStrokeStyle(int style) throws RemoteException {
    g.setStrokeStyle(style);
  }

  @Override
  public void drawRegionRop(
      Image src, int sx, int sy, int w, int h, int x, int y, int anchor, int rop)
      throws RemoteException {
    g.drawRegionRop(src, sx, sy, w, h, x, y, anchor, rop);
  }

  @Override
  public void drawRegionRop(
      Image src, int sx, int sy, int w, int h, int transform, int x, int y, int anchor, int rop)
      throws RemoteException {
    g.drawRegionRop(src, sx, sy, w, h, transform, x, y, anchor, rop);
  }

  @Override
  public void drawRegion(
      Image src, int sx, int sy, int w, int h, int transform, int x, int y, int anchor)
      throws RemoteException {
    g.drawRegion(src, sx, sy, w, h, transform, x, y, anchor);
  }

  @Override
  public void drawImage(Image src, int x, int y, int anchor) throws RemoteException {
    g.drawImage(src, x, y, anchor);
  }

  @Override
  public void drawLine(int x0, int y0, int x1, int y1) throws RemoteException {
    g.drawLine(x0, y0, x1, y1);
  }

  @Override
  public void drawArc(int x, int y, int width, int height, int startAngle, int arcAngle)
      throws RemoteException {
    g.drawArc(x, y, width, height, startAngle, arcAngle);
  }

  @Override
  public void fillArc(int x, int y, int width, int height, int startAngle, int arcAngle)
      throws RemoteException {
    g.fillArc(x, y, width, height, startAngle, arcAngle);
  }

  @Override
  public void drawRoundRect(int x, int y, int width, int height, int arcWidth, int arcHeight)
      throws RemoteException {
    g.drawRoundRect(x, y, width, height, arcWidth, arcHeight);
  }

  @Override
  public void drawRect(int x, int y, int width, int height) throws RemoteException {
    g.drawRect(x, y, width, height);
  }

  @Override
  public void fillRect(int x, int y, int w, int h) throws RemoteException {
    g.fillRect(x, y, w, h);
  }

  @Override
  public void copyArea(int sx, int sy, int w, int h, int x, int y, int anchor)
      throws RemoteException {
    g.copyArea(sx, sy, w, h, x, y, anchor);
  }

  @Override
  public Font getFont() throws RemoteException {
    return g.getFont();
  }

  @Override
  public void setFont(Font f) throws RemoteException {
    g.setFont(f);
  }

  @Override
  public void translate(int x, int y) throws RemoteException {
    g.translate(x, y);
  }

  @Override
  public int getTranslateX() throws RemoteException {
    return g.getTranslateX();
  }

  @Override
  public int getTranslateY() throws RemoteException {
    return g.getTranslateY();
  }

  @Override
  public void setColor(int rgb) throws RemoteException {
    g.setColor(rgb);
  }

  @Override
  public void setColor(int red, int green, int blue) throws RemoteException {
    g.setColor(red, green, blue);
  }

  @Override
  public void refresh() throws RemoteException {
    g.refresh();
  }

  @Override
  public void clear() throws RemoteException {
    g.clear();
  }

  @Override
  public int getWidth() throws RemoteException {
    return g.getWidth();
  }

  @Override
  public int getHeight() throws RemoteException {
    return g.getHeight();
  }

  @Override
  public byte[] getDisplay() throws RemoteException {
    return g.getDisplay();
  }

  @Override
  public byte[] getHWDisplay() throws RemoteException {
    return g.getHWDisplay();
  }

  @Override
  public void setContrast(int contrast) throws RemoteException {
    g.setContrast(contrast);
  }

  @Override
  public void bitBlt(
      byte[] src, int sw, int sh, int sx, int sy, int dx, int dy, int w, int h, int rop)
      throws RemoteException {
    g.bitBlt(src, sw, sh, sx, sy, dx, dy, w, h, rop);
  }

  @Override
  public void bitBlt(
      byte[] src,
      int sw,
      int sh,
      int sx,
      int sy,
      byte[] dst,
      int dw,
      int dh,
      int dx,
      int dy,
      int w,
      int h,
      int rop)
      throws RemoteException {
    g.bitBlt(src, sw, sh, sx, sy, dst, dw, dh, dx, dy, w, h, rop);
  }

  @Override
  public void setAutoRefresh(boolean on) throws RemoteException {
    g.setAutoRefresh(on);
  }

  @Override
  public int setAutoRefreshPeriod(int period) throws RemoteException {
    return g.setAutoRefreshPeriod(period);
  }
}
/**
 * @author Asher Wright
 * @version 2.0 ECSE 211 CTF Robot This class sets up the classes for Finding the objects, and calls
 *     them. It also initializes the sensors and the motors.
 */
public class Controller {
  // variables for WiFi module
  // *** INSTRUCTIONS ***
  // SERVER_IP: the IP address of the computer running the server application
  private static final String SERVER_IP =
      "192.168.10.200"; // "192.168.10.19";//"192.168.10.200";//"192.168.10.116";//"192.168.10.120";//"192.168.10.116"; //YAN or Rahul: "192.168.43.118";
  private static final int TEAM_NUMBER = 14;

  // robot dimension constants
  public static final double ROBOT_CENTRE_TO_LIGHTLOCALIZATION_SENSOR = 10.6;
  public static final double WHEEL_RADIUS = 2.09;
  public static final double TRACK = 15.15;
  public static final double TILE_WIDTH = 30.4;

  // Static Resources:
  // LCD screen
  private static TextLCD LCD = LocalEV3.get().getTextLCD();
  // Left motor connected to output A
  // Right motor connected to output D
  // Ultrasonic sensor port connected to input S1
  // Color sensor port connected to input S2
  private static final EV3LargeRegulatedMotor leftMotor =
      new EV3LargeRegulatedMotor(LocalEV3.get().getPort("D"));
  private static final EV3LargeRegulatedMotor rightMotor =
      new EV3LargeRegulatedMotor(LocalEV3.get().getPort("C"));
  // the two arm motors for capturing the block

  private static final EV3LargeRegulatedMotor verticalArmMotor =
      new EV3LargeRegulatedMotor(LocalEV3.get().getPort("B"));
  private static final EV3LargeRegulatedMotor horizontalArmMotor =
      new EV3LargeRegulatedMotor(LocalEV3.get().getPort("A"));

  // sensor ports

  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);
    }
  }
}
Пример #9
0
public class Odometer extends Thread {
  // robot position
  private double x, y, theta, thetaDeg;

  // constant
  public static double WB = Lab2.TRACK;
  public static double WR = Lab2.WHEEL_RADIUS;

  // variables
  public static int lastTachoL; // Tacho L at last sample
  public static int lastTachoR; // Tacho R at last sample
  public static int nowTachoL; // Current tacho L
  public static int nowTachoR; // Current tacho R

  // Resources
  static TextLCD t = LocalEV3.get().getTextLCD();
  private EV3LargeRegulatedMotor leftMotor; // L
  private EV3LargeRegulatedMotor rightMotor; // L

  // odometer update period, in ms
  private static final long ODOMETER_PERIOD = 25;

  // lock object for mutual exclusion
  private Object lock;

  // default constructor
  public Odometer(EV3LargeRegulatedMotor leftmotor, EV3LargeRegulatedMotor rightmotor) {
    x = 0.0;
    y = 0.0;
    theta = 0.0;
    lock = new Object();
    this.leftMotor = leftmotor;
    this.rightMotor = rightmotor;
  }

  // run method (required for Thread)
  public void run() {
    long updateStart, updateEnd;
    double distL, distR, deltaD, deltaT, dX, dY;

    while (true) {
      updateStart = System.currentTimeMillis();
      // put (some of) your odometer code here

      nowTachoL = leftMotor.getTachoCount(); // get tacho counts
      nowTachoR = rightMotor.getTachoCount();
      distL = Math.PI * WR * (nowTachoL - lastTachoL) / 180; // compute
      // wheel
      distR = Math.PI * WR * (nowTachoR - lastTachoR) / 180; // displacements
      lastTachoL = nowTachoL; // save tacho counts for next iteration
      lastTachoR = nowTachoR;
      deltaD = 0.5 * (distL + distR); // compute vehicle displacement
      deltaT = (distL - distR) / WB; // compute change in heading

      synchronized (lock) {
        // don't use the variables x, y, or theta anywhere but here!

        theta += deltaT;
        if (theta >= Math.PI * 2) {
          theta = theta - Math.PI * 2;
        }
        dX = deltaD * Math.sin(theta); // compute X component of
        // displacement
        dY = deltaD * Math.cos(theta); // compute Y component of
        // displacement
        x = x + dX;
        y = y + dY;
        thetaDeg = theta * 180 / Math.PI;
      }

      // this ensures that the odometer only runs once every period
      updateEnd = System.currentTimeMillis();
      if (updateEnd - updateStart < ODOMETER_PERIOD) {
        try {
          Thread.sleep(ODOMETER_PERIOD - (updateEnd - updateStart));
        } catch (InterruptedException e) {
          // there is nothing to be done here because it is not
          // expected that the odometer will be interrupted by
          // another thread
        }
      }
    }
  }

  // accessors
  public void getPosition(double[] position, boolean[] update) {
    // ensure that the values don't change while the odometer is running
    synchronized (lock) {
      if (update[0]) position[0] = x;
      if (update[1]) position[1] = y;
      if (update[2]) position[2] = thetaDeg;
    }
  }

  public double getX() {
    double result;

    synchronized (lock) {
      result = x;
    }

    return result;
  }

  public double getY() {
    double result;

    synchronized (lock) {
      result = y;
    }

    return result;
  }

  public double getTheta() {
    double result;

    synchronized (lock) {
      result = theta;
    }

    return result;
  }

  // mutators
  public void setPosition(double[] position, boolean[] update) {
    // ensure that the values don't change while the odometer is running
    synchronized (lock) {
      if (update[0]) x = position[0];
      if (update[1]) y = position[1];
      if (update[2]) theta = position[2];
    }
  }

  public void setX(double x) {
    synchronized (lock) {
      this.x = x;
    }
  }

  public void setY(double y) {
    synchronized (lock) {
      this.y = y;
    }
  }

  public void setTheta(double theta) {
    synchronized (lock) {
      this.theta = theta;
    }
  }
}
Пример #10
0
 public void driveAround() {
   while (LocalEV3.get().getKeys().waitForAnyPress() != LocalEV3.get().getKeys().ID_ESCAPE) {}
 }
Пример #11
0
  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);
  }
public class ColorSensorThreaded extends Block {
  public final int CONSECUTIVE_READINGS_REQUIREMENT = 2;
  public final int SAMPLESIZE = 20;
  public final int BACKGROUND_COLOR_ID = Color.BROWN;
  public final int HIGHEST_SIGNAL_COLOR_ID = 5;
  public EV3ColorSensor colorSensor;
  public Port s1 = LocalEV3.get().getPort("S1");
  public int lastDetectedColorId = -1;
  public int lastRegisteredColorId = BACKGROUND_COLOR_ID;
  public int readings;
  public boolean stopped = false;
  public java.lang.Thread sensorThread;
  private ArrayList<String> log = new ArrayList<String>();
  private long previousLogging;

  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);
  }

  public void stop() {
    stopped = true;
    writeLogToFile();
  }

  public void sleep() {
    try {
      Thread.sleep(5);
    } catch (InterruptedException e) {
      e.printStackTrace();
    }
  }

  public void writeLogToFile() {
    try {
      PrintWriter writer = new PrintWriter("readerlog.txt", "UTF-8");
      writer.println("Battery voltage: " + lejos.hardware.Battery.getVoltage());
      for (String string : log) {
        writer.println(string);
      }
      writer.close();
    } catch (FileNotFoundException e) {
      // TODO Auto-generated catch block
      e.printStackTrace();
    } catch (UnsupportedEncodingException e) {
      // TODO Auto-generated catch block
      e.printStackTrace();
    }
  }
}