public void run() {
leftMotor.setAcceleration(2000);
rightMotor.setAcceleration(2000);
distMid = getFilteredData();
sensorMotor.rotate(45);
Delay.msDelay(500);
distLeft = getFilteredData();
sensorMotor.rotate(-90);
Delay.msDelay(500);
distRight = getFilteredData();
sensorMotor.rotate(45);
if (distLeft > distMid && distLeft > distRight) {
select = 0;
} else if (distRight >= distMid && distRight >= distLeft) {
select = 2; // turn right
}
if (select == 0) {
leftMotor.setSpeed(ROTATE_SPEED);
rightMotor.setSpeed(ROTATE_SPEED);
sensorMotor.setAcceleration(1000);
angleSave = odometer.getAng();
leftMotor.rotate(-convertAngle(2.2, 15, 89.8), true);
rightMotor.rotate(convertAngle(2.2, 15, 89.8), false);
sensorMotor.rotate(-85);
while (safe == false || collide() == false) {
if (getFilteredData() > 100 && filterControl < FILTER_OUT) {
// bad value, do not set the distance var, however do increment the filter value
filterControl++;
} else if (getFilteredData() > 100) {
// set distance to this.distance
distance = 100;
} else {
// distance went below 100, therefore reset everything.
filterControl = 0;
distance = getFilteredData();
}
perror = distance - bandCenter;
amperror = perror * proportion;
if (distance > 60) {
// if distance >80, the robot is about to do a U-turn
leftMotor.setSpeed(maxSpeed);
rightMotor.setSpeed(motorStraight);
// right wheel will speed up in order to turn left
leftMotor.forward();
rightMotor.forward();
}
/* else if(distance<8){
// if distance<8, the robot is going to hit the wall
leftMotor.setSpeed(motorStraight+50);
rightMotor.setSpeed(motorStraight+50);
leftMotor.forward();
rightMotor.backward();
// right motor rolls backwards to respond.
}
*/
else {
// general proportion error correction
rightMotor.setSpeed(motorStraight);
if ((motorStraight + amperror) > maxSpeed) {
leftMotor.setSpeed(maxSpeed);
}
// left wheel remains constant speed
else {
leftMotor.setSpeed((int) (motorStraight + amperror));
}
// right wheel will continuously correcting its speed according to errors.
leftMotor.forward();
rightMotor.forward();
}
if (CloseTo(rightDegreeHelper(angleSave, odometer.getAng()), -90)) {
sensorMotor.rotate(85);
safe = true;
}
}
}
if (select == 2) {
leftMotor.setSpeed(ROTATE_SPEED);
rightMotor.setSpeed(ROTATE_SPEED);
sensorMotor.setAcceleration(1000);
angleSave = odometer.getAng();
leftMotor.rotate(convertAngle(2.2, 15, 89.8), true);
rightMotor.rotate(-convertAngle(2.2, 15, 89.8), false);
sensorMotor.rotate(85);
while (safe == false || collide() == false) {
if (getFilteredData() > 100 && filterControl < FILTER_OUT) {
// bad value, do not set the distance var, however do increment the filter value
filterControl++;
} else if (getFilteredData() > 100) {
// set distance to this.distance
distance = 100;
} else {
// distance went below 100, therefore reset everything.
filterControl = 0;
distance = getFilteredData();
}
perror = distance - bandCenter;
amperror = perror * proportion;
if (distance > 60) {
// if distance >80, the robot is about to do a U-turn
leftMotor.setSpeed(motorStraight);
rightMotor.setSpeed(maxSpeed);
// right wheel will speed up in order to turn left
leftMotor.forward();
rightMotor.forward();
}
/* else if(distance<10){
// if distance<8, the robot is going to hit the wall
rightMotor.setSpeed(motorStraight+50);
rightMotor.setSpeed(motorStraight+50);
rightMotor.forward();
leftMotor.backward();
// right motor rolls backwards to respond.
}
*/
else {
// general proportion error correction
leftMotor.setSpeed(motorStraight);
if ((motorStraight + amperror) > maxSpeed) {
rightMotor.setSpeed(maxSpeed);
}
// left wheel remains constant speed
else {
rightMotor.setSpeed((int) (motorStraight + amperror));
}
// right wheel will continuously correcting its speed according to errors.
leftMotor.forward();
rightMotor.forward();
}
if (CloseTo(leftDegreeHelper(angleSave, odometer.getAng()), 90)) {
sensorMotor.rotate(-85);
safe = true;
}
}
}
if (collide() == true) {
leftMotor.stop(true);
rightMotor.stop(false);
leftMotor.rotate(convertDistance(2.2, -10), true);
rightMotor.rotate(convertDistance(2.2, -10), false);
}
}
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);
}
