// Author: Mark Lanthier (SN:100000001)
import com.cyberbotics.webots.controller.DistanceSensor;
import com.cyberbotics.webots.controller.Motor;
import com.cyberbotics.webots.controller.Robot;
public class Lab2Controller {
// Various modes that the robot may be in
static final byte STRAIGHT = 0;
static final byte SPIN_LEFT = 1;
static final byte PIVOT_RIGHT = 2;
static final byte CURVE_LEFT = 3;
static final byte CURVE_RIGHT = 4;
static final double MAX_SPEED = 5; // maximum speed of the epuck robot
// Inclusive
public static boolean inRangeOf(int lowerBound, int upperBound, int value) {
return lowerBound <= value && value <= upperBound;
}
// Inclusive (0 = too low, 1 = too high, 2 = just right)
public static int rangeCompare(int lowerBound, int upperBound, int value) {
if (value < lowerBound)
return 0;
if (value > upperBound)
return 1;
return 2;
}
public static void main(String[] args) {
Robot robot = new Robot();
int timeStep = (int) Math.round(robot.getBasicTimeStep());
// Get the motors
Motor leftMotor = robot.getMotor("left wheel motor");
Motor rightMotor = robot.getMotor("right wheel motor");
leftMotor.setPosition(Double.POSITIVE_INFINITY);// indicates continuous rotation servo
rightMotor.setPosition(Double.POSITIVE_INFINITY);// indicates continuous rotation servo
// Get and enable the sensors
DistanceSensor leftAheadSensor = robot.getDistanceSensor("ps7");
DistanceSensor rightAheadSensor = robot.getDistanceSensor("ps0");
DistanceSensor rightAngledSensor = robot.getDistanceSensor("ps1");
DistanceSensor rightSideSensor = robot.getDistanceSensor("ps2");
leftAheadSensor.enable(timeStep);
rightAheadSensor.enable(timeStep);
rightAngledSensor.enable(timeStep);
rightSideSensor.enable(timeStep);
final int FAR_THRESH = 80;
final int CLOSE_THRESH = 1000;
// Initialize the logic variable for turning
byte currentMode = STRAIGHT;
double leftSpeed, rightSpeed;
while (robot.step(timeStep) != -1) {
// SENSE: Read the distance sensors
double rightSideValue = rightSideSensor.getValue();
boolean rightTooFar = rightSideValue < FAR_THRESH;
boolean rightTooClose = rightSideValue > CLOSE_THRESH;
boolean rightJustNice = !rightTooFar && !rightTooClose;
System.out.println(rightSideValue);
// THINK: Check for obstacle and decide how we need to turn
switch (currentMode) {
case STRAIGHT:
System.out.println("STRAIGHT");
if (rightTooFar)
currentMode = CURVE_RIGHT;
else if (rightTooClose)
currentMode = CURVE_LEFT;
break;
case CURVE_RIGHT:
System.out.println("CURVE RIGHT");
if (rightTooClose)
currentMode = CURVE_LEFT;
else if (rightJustNice)
currentMode = STRAIGHT;
break;
case PIVOT_RIGHT:
System.out.println("PIVOT RIGHT");
break;
case SPIN_LEFT:
System.out.println("SPIN LEFT");
break;
case CURVE_LEFT:
System.out.println("CURVE LEFT");
if (rightTooFar)
currentMode = CURVE_RIGHT;
else if (rightJustNice)
currentMode = STRAIGHT;
break;
}
// REACT: Move motors accordingly
switch (currentMode) {
case SPIN_LEFT:
leftMotor.setVelocity(-1 * MAX_SPEED * 0.5);
rightMotor.setVelocity(MAX_SPEED * 0.5);
break;
case PIVOT_RIGHT:
leftMotor.setVelocity(MAX_SPEED * 0.5);
rightMotor.setVelocity(0);
break;
case CURVE_LEFT:
leftMotor.setVelocity(MAX_SPEED * 0.75);
rightMotor.setVelocity(MAX_SPEED);
break;
case CURVE_RIGHT:
leftMotor.setVelocity(MAX_SPEED);
rightMotor.setVelocity(MAX_SPEED * 0.75);
break;
default: // This handles the STRAIGHT case
leftMotor.setVelocity(MAX_SPEED);
rightMotor.setVelocity(MAX_SPEED);
break;
}
}
}
}