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);
}
}
/** 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);
}
}
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);
}
}
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);
}
}
public Buttoni() {
port3 = LocalEV3.get().getPort("S2");
tsensor = new EV3TouchSensor(port3);
}
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);
}
}
}
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;
}
}
}
public void driveAround() {
while (LocalEV3.get().getKeys().waitForAnyPress() != LocalEV3.get().getKeys().ID_ESCAPE) {}
}
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();
}
}
}