// 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);
}
}
/**
* 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);
}
}
// 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);
}
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 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;
}
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;
}
}
}
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);
}
}
/**
* 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();
}
/** 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 void align(DifferentialPilot pilot, int performedRotation) {
pilot.rotate(-performedRotation);
pilot.rotate(-45);
}
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