/**
* Rotates the robot a specified number of degrees left or right.
*
* @param turnDegrees the degree of the turn.
* @param turnLeft true to turn left, false to turn right.
*/
public void rotation(int turnDegrees, boolean turnLeft) {
if (turnLeft == true) {
pilot.rotate(turnDegrees);
} else {
pilot.rotate(-turnDegrees);
}
}
public int[] getAngledDistances(DifferentialPilot pilot, int angle, int[] prevVals) {
// const
int sleepTime = 350;
// action info
LCD.clear(0);
LCD.drawString("Finding direction", 0, 0);
// get central dist
prevVals[1] += getFastMeasurement();
LCD.drawInt(prevVals[1], 5, 1);
// rotate, ping, wait a bit
pilot.rotate(angle);
// read left distance
prevVals[0] += getFastMeasurement();
LCD.drawInt(prevVals[0], 0, 1);
// rotate, ping, wait a bit
pilot.rotate(-2 * angle);
// read right distance
prevVals[2] += getFastMeasurement();
LCD.drawInt(prevVals[2], 10, 1);
pilot.rotate(angle);
return prevVals;
}
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 drawSquare(float length) {
byte direction_l = 1;
if (length < 0) {
direction_l = -1;
length = -length;
}
for (int i = 0; i < 4; i++) {
p.travel(length);
p.rotate(direction_l * 90);
}
}
/** Default constructor, initializes the motors and sensors */
public MazeSolver() {
// Set up motors and sensors
myPilot = new DifferentialPilot(wheelDiam, axleLength, Motor.B, Motor.A, true);
myFrontSensor = new LightSensor(SensorPort.S1);
myRightSensor = new LightSensor(SensorPort.S2);
myCompass = new CompassSensor(SensorPort.S3);
// Set rotate speed
myPilot.setRotateSpeed(rotateSpeed);
myPilot.setAcceleration(70);
// Calibrate
doCalibration();
}
/**
* If an abyss is detected, the robot will rotate 10 degrees, until the light sensor sees normal
* ground again.
*/
@Override
public void action() {
System.out.println("Abys");
suppressed = false;
while (lightSensor.getLightValue() < threshold && !suppressed) {
pilot.rotate(10, true);
if (!pilot.isMoving()) {
Settings.whipAndBridgeCounter++;
}
Delay.msDelay(10);
}
while (pilot.isMoving() && !suppressed) {
Thread.yield();
}
pilot.stop();
}
private boolean moveStraight(boolean forward, int distance) {
if (forward) {
if (distance == 0) {
pilot.forward();
return true;
} else {
pilot.travel(distance);
return true;
}
} else {
if (distance == 0) {
pilot.backward();
return true;
} else {
pilot.travel(-distance);
return true;
}
}
}
private boolean turn(boolean right, int radius) {
if (right) {
if (radius == 0) {
pilot.rotateRight();
return true;
} else {
pilot.rotate(radius);
return true;
}
} else {
if (radius == 0) {
pilot.rotateLeft();
return true;
} else {
pilot.rotate(-radius);
return true;
}
}
}
/**
* Stops the robot when the ultrasonic sensor detects a certain distance.
*
* @param stopDistance as long as the current distance is less than this stop distance, the robot
* will keep moving. Once the current distance is larger than the stop distance, the function
* returns.
* @return the current distance upon stopping.
*/
private boolean stopBeforeEdge() {
int currentDistance = 0;
int lightLevelReading;
int distanceToObject;
boolean edgeIsSafe = false;
do {
lightLevelReading = lightSensorDown.readValue();
Motor.A.rotate(centerToEdgeDistance, true);
Motor.C.rotate(centerToEdgeDistance, true);
} while ((lightLevelReading > BLACK_WHITE_THRESHOLD)
&& (Motor.A.isMoving() && Motor.C.isMoving()));
pilot.stop();
lightLevelReading = lightSensorDown.readValue();
/*
distanceToObject = ultrasonicSensor.getDistance();
if(distanceToObject < 20)
{
edgeIsSafe = true;
Motor.A.setSpeed(1000);
Motor.C.setSpeed(1000);
pilot.setRotateSpeed(1000);
pilot.travel(280);
Delay.msDelay(10000);
return(edgeIsSafe);
}
if(lightLevelReading < BLACK_WHITE_THRESHOLD)
{
System.out.println("Unsafe edge");
edgeIsSafe = false;
return(edgeIsSafe);
}
else if (lightLevelReading > BLACK_WHITE_THRESHOLD)
{
System.out.println("Safe edge");
edgeIsSafe = true;
safeEdgeCount++;
return(edgeIsSafe);
}
*/
return (edgeIsSafe);
}
/** The robot moves along the Wall and turns to the right, if the wall is too far away. */
@Override
public void action() {
suppressed = false;
Settings.motorAAngle = Settings.SENSOR_RIGHT;
while (!suppressed
&& !((Settings.TOUCH_L.isPressed() || Settings.TOUCH_R.isPressed()))
&& Settings.LIGHT.getLightValue() < 80) {
if (sonic.getDistance() > (distanceToWall + 10)) {
pilot.arc(-15, -90, true);
} else if (sonic.getDistance() <= distanceToWall) {
pilot.arc(40, 20, true);
} else if (sonic.getDistance() > distanceToWall) {
pilot.arc(-60, -20, true);
}
}
if (Settings.LIGHT.getLightValue() >= 80) {
pilot.stop();
System.out.println("LINE Detected");
Settings.afterSlider = true;
Settings.motorAAngle = Settings.SENSOR_FRONT;
Settings.PILOT.setTravelSpeed(Settings.PILOT.getMaxTravelSpeed() * 0.15);
}
}
/** Turns the robot left using compass */
private void turnLeft() {
System.out.println("L");
/*
* // Stop momentarily myPilot.stop();
*
* // Get current bearing float x = myCompass.getDegrees(); float y = (x
* + 90f) % 360;
*
* // Get us within a threshold of the degree that we want while (x < y
* - 3) { myPilot.rotate(-5); x = myCompass.getDegrees();
* System.out.println("Bearing: " + x); }
*/
// Turn Left
goBack();
// goBack();
myPilot.rotate(76);
goForward(2, travelDist);
}
private boolean moveArc(boolean forward, boolean right, int distance, int radius) {
if (forward) {
if (right) {
if (distance == 0 && radius == 0) {
pilot.arcForward(-DEFAULT_RADIUS);
return true;
} else {
pilot.arc(-radius, FORWARD_ANGLE);
return true;
}
} else {
if (distance == 0 && radius == 0) {
pilot.arcForward(DEFAULT_RADIUS);
return true;
} else {
pilot.arc(radius, FORWARD_ANGLE);
return true;
}
}
} else {
if (right) {
if (distance == 0 && radius == 0) {
pilot.arcBackward(-DEFAULT_RADIUS);
return true;
} else {
pilot.arc(-radius, BACKWARD_ANGLE);
return true;
}
} else {
if (distance == 0 && radius == 0) {
pilot.arcBackward(DEFAULT_RADIUS);
return true;
} else {
pilot.arc(radius, BACKWARD_ANGLE);
return true;
}
}
}
}
private int waitForReading() {
int lightLevelReading;
int distanceToObject;
do {
try {
Thread.sleep(100);
} catch (InterruptedException ex) {
}
lightLevelReading = lightSensorDown.readValue();
} while (lightLevelReading > BLACK_WHITE_THRESHOLD);
distanceToObject = ultrasonicSensor.getDistance();
if (distanceToObject < 20) {
Motor.A.setSpeed(1000);
Motor.C.setSpeed(1000);
pilot.travel(300);
Delay.msDelay(10000);
}
return (lightLevelReading);
}
// circle method: traces a semi-circle and a quarter circle
// and takes two DifferentialPilot objects as method input
public static void circle(DifferentialPilot Pilot1, DifferentialPilot Pilot2) {
// Semi-Circle
Pilot1.setTravelSpeed(5); // first drawing the arc for the semi-circle
Pilot1.arc(36, 180); // robot arcs left for 180 degrees, guided by a 36 inch radius
Pilot1.stop();
Delay.msDelay(300);
Pilot1.setRotateSpeed(30);
Pilot1.rotate(90); // robot makes a 90 degree left turn
Pilot1.stop();
Delay.msDelay(300);
Pilot1.setTravelSpeed(5);
Pilot1.travel(
72); // here, we're closing up the semi-circle (that is, tracing diameter of the circle)
Delay.msDelay(1300);
// Quarter-Circle
Pilot2.setTravelSpeed(5);
Pilot2.arc(12, 90); // robot arcs right for 90 degree (so forms 1/4 of circle)
Pilot2.stop();
Delay.msDelay(300);
// loop to close up the quarter circle
for (int l = 1; l <= 2; l++) {
Pilot2.setRotateSpeed(30);
Pilot2.rotate(90); // robot makes a 90 degree right turn
Pilot2.stop();
Delay.msDelay(300);
Pilot2.setTravelSpeed(5);
Pilot2.travel(12); // robot goes forward for 12 in (the radius of circle)
Pilot2.stop();
Delay.msDelay(300);
}
}
// polygon method: traces a triangle and a pentagon, and takes a DifferentialPilot object as
// method input
public static void polygon(DifferentialPilot Pilot1) {
// Triangle: having three sides, so for-loop goes from 1 to 3
for (int i = 1; i <= 3; i++) {
Pilot1.setTravelSpeed(5); // setting the travel and rotation speed for the robot
Pilot1.setRotateSpeed(30);
Pilot1.travel(24); // robot travels 2 feet forward
Pilot1.stop();
Delay.msDelay(300);
Pilot1.rotate(120); // robot rotates left to form a 60 degree interior angle
Pilot1.stop();
Delay.msDelay(300);
}
Delay.msDelay(1300);
// Pentagon: having five sides, so for-loop goes from 1 to 5
for (int i = 1; i <= 5; i++) {
Pilot1.setTravelSpeed(5); // setting the travel and rotation speed for the robot
Pilot1.setRotateSpeed(30);
Pilot1.travel(24); // robot travels 2 feet forward
Pilot1.stop();
Delay.msDelay(300);
Pilot1.rotate(72); // robot rotates left to form 108 interior angle of pentagon
Pilot1.stop();
Delay.msDelay(300);
}
Delay.msDelay(1300);
}
/** Set the velocity of the robot. Wheel diameter units per second */
@Override
public void setVelocity(double speed) {
pilot.setTravelSpeed(speed);
}
private void goBack() {
System.out.println("B");
// Move some more, and return right away
myPilot.travel(travelDist * -2.1);
}
/**
* Returns a boolean based on the robot moving.
*
* @return boolean, True if robot is moving
*/
@Override
public boolean isMoving() {
return pilot.isMoving();
}
private boolean stop() {
pilot.stop();
return true;
}
public static void main(String[] args) throws Exception {
DifferentialPilot pilot = new DifferentialPilot(56f, 126f, Motor.D, Motor.A);
pilot.setTravelSpeed(150);
Brick brick = BrickFinder.getDefault();
Port s1 = brick.getPort("S1"); //
Port s4 = brick.getPort("S4"); //
TextLCD lcd = brick.getTextLCD();
NXTLightSensor lysSensor = new NXTLightSensor(s1); // NXT LYS
EV3ColorSensor fargeSensor = new EV3ColorSensor(s4); // EV3 LYS
// COLOR-----------------------------------------------------------------
SampleProvider fargeSample = fargeSensor.getColorIDMode();
float[] colorVerdi = new float[fargeSample.sampleSize()];
// ----------------------------------------------------------------------
// LYS-------------------------------------------------------------------
SampleProvider lysSample = lysSensor;
float[] lysVerdi = new float[lysSample.sampleSize()];
// ----------------------------------------------------------------------
Boolean move = true; // Sett til false hvis roboten ikke skal kjøre videre
// System.out.println("start lokke");
while (!Button.ESCAPE.isDown()) { // Kjører så lenge vi ikke trykker på exit-knappen
// Kjør framover hvis vi ikke allerede gjør det
if (!pilot.isMoving() && move) {
pilot.forward();
}
lysSensor.fetchSample(lysVerdi, 0); // LYS
float lys = lysVerdi[0];
fargeSample.fetchSample(colorVerdi, 0); // COLOR
// float color = colorVerdi[0];
int color = fargeSensor.getColorID(); // Fungerer dette?
lcd.clear();
lcd.drawString("NXT " + lys, 0, 0);
lcd.drawString("EV3 " + color, 0, 1);
if (color != Color.BLACK) {
pilot.rotate(-10); // VENSTRE
} else if (lys < 48) {
// pilot.arcForward(-150);
pilot.rotate(10);
}
// Elsen under trengs ikke, da forward kjøres på starten uansett
else {
// pilot.forward();
if (!pilot.isMoving()) {
pilot.forward();
}
}
} // while
System.out.println("AVSLUTTET");
} // main
/** Go forward method */
private void goForward(double numTimes, double dist) {
System.out.println("F");
// Move some more, and return right away
myPilot.travel(dist * numTimes);
}
private void align(DifferentialPilot pilot, int performedRotation) {
pilot.rotate(-performedRotation);
pilot.rotate(-45);
}
// Called once after isFinished returns true
protected void end() {
p.stop();
drivetrain.tankDrive(0, 0);
drivetrain.disableControl();
}
/**
* Returns the velocity of the robot.
*
* @return double, the velocity of the robot in wheel diameter units per second
*/
@Override
public double getVelocity() {
return pilot.getTravelSpeed();
}