Commit 43aa8171 authored by Enniwhere's avatar Enniwhere
Browse files

Finished the Escape behavior

parent 308f1610
Pipeline #3166 skipped
......@@ -9,8 +9,9 @@ class Escape extends Thread
{
private SharedCar car = new SharedCar();
private int power = 70, ms = 500;
UltrasonicSensor sonar = new UltrasonicSensor(SensorPort.S1);
private int power = 70, ms = 500, backwards_time = 2000;
TouchSensor leftBumper = new TouchSensor(SensorPort.S2);
TouchSensor rightBumper = new TouchSensor(SensorPort.S3);
int frontDistance, leftDistance, rightDistance;
int stopThreshold = 30;
......@@ -24,35 +25,30 @@ class Escape extends Thread
{
while (true)
{
// Monitor the distance in front of the car and stop
// when an object gets to close
frontDistance = sonar.getDistance();
while ( frontDistance >= stopThreshold )
{
car.noCommand();
frontDistance = sonar.getDistance();
// If both bumpers are pressed drive back and turn left
if (leftBumper.isPressed() && rightBumper.isPressed()){
car.backward(power, power);
Delay.msDelay(backwards_time);
car.forward(0,power);
Delay.msDelay(ms);
car.stop();
}
car.stop();
Delay.msDelay(ms);
// Get the distance to the left
car.forward(0, power);
Delay.msDelay(ms);
leftDistance = sonar.getDistance();
// Get the distance to the right
car.backward(0, power);
Delay.msDelay(ms);
car.forward(power, 0);
Delay.msDelay(ms);
rightDistance = sonar.getDistance();
// Turn in the direction with most space in front of the car
if ( leftDistance > rightDistance ){
car.backward(power, 0);
// If left bumper is pressed turn right
else if (leftBumper.isPressed()){
car.backward(0, power);
Delay.msDelay(ms);
car.forward(0, power);
car.stop();
}
// If right bumper is pressed turn left
else if (rightBumper.isPressed()) {
car.backward(power, 0);
Delay.msDelay(ms);
car.stop();
}
// Else yield
else {
car.noCommand();
}
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment