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