public static void main(String[] args) throws InterruptedException {
// Wait for bluetooth/usb connection to computer.
RConsole.open();
int arenaLength = 200;
int arenaWidth = 150;
int cellSize = 25;
// Set up the world map.
// Length, width, cellHeight, cellWidth.
worldMap = new WorldMap(arenaLength, arenaWidth, cellSize, cellSize);
// Set up the robot and pass it the world map.
robot = new Robot(worldMap);
// Wait to start
System.out.println("Press any button to start");
Button.waitForAnyPress();
Output.clear();
BumperMonitor bm = new BumperMonitor(robot);
bm.start();
// Main robot loop.
// Keeps going till fully explored.
while (!robot.getMap().fullyExplored()) {
if (!Robot.takecontrol) {
// Print the map before every movement.
Output.printMapLCD(robot);
// Check for obstacles to the left, right and front of robot.
robot.checkSurroundings();
// Choose a direction to move in.
Robot.Direction directionChosen = robot.chooseACell();
// Turn in the chosen direction.
robot.turnRobotToFace(directionChosen);
// Move forwards.
robot.moveForwards();
}
}
// Print out the finished world map.
Output.println("Final world map");
Output.printMapLCD(robot);
/*
Wait before ending.
This is done because the world map kept not being
printed correctly for some reason and this fixes it.
*/
RConsole.println("\nPress any button to end.");
Button.waitForAnyPress();
RConsole.close();
}
public void timedOut() {
RConsole.println("" + odo.getAngle() + ", " + linePoller.getColourVal());
// RConsole.println("" + odo.getX() + ", " + odo.getY() + ", " + odo.getAngle());
// RConsole.println("");
// RConsole.println("" + Controller.task);
// RConsole.println("");
// RConsole.println("");
// RConsole.println("X: " + (int)odo.getX());
// RConsole.println("Y: " + (int)odo.getY());
// RConsole.println("Angle: " + (int)odo.getAngle());
// RConsole.println("");
// RConsole.println("");
// RConsole.println("Search Zone: (" + Controller.ourZoneLL_X + ", " + Controller.ourZoneLL_Y +
// ") to (" + Controller.ourZoneUR_X + ", " + Controller.ourZoneUR_Y + ")");
// RConsole.println("Flag coulour: " + Controller.ourFlagColour);
//
//
// switch (Controller.task){
// case NAVIGATING:
// break;
// case SEARCHING:
// break;
// default:
// break;
// }
}
private static void init() {
RConsole.openBluetooth(15000);
Robot.L_LEG_MOTOR.regulateSpeed(true);
Robot.R_LEG_MOTOR.regulateSpeed(true);
Robot.HEAD_MOTOR.regulateSpeed(true);
RobotStatus.setAutoUpdateStatus(true);
DistanceMonitor.getInstance().startMonitoring();
}
public static void main(String[] args) throws InterruptedException {
OpticalDistanceSensor us = new OpticalDistanceSensor(SensorPort.S1);
final DifferentialPilot pilot =
new DifferentialPilot(
RobotConstants.WHEEL_DIAMETER / 10,
RobotConstants.TRACK_WIDTH / 10,
RobotConstants.leftMotor,
RobotConstants.rightMotor,
true);
final ColorSensor s = new ColorSensor(SensorPort.S3);
RConsole.openBluetooth(60000);
System.setOut(new PrintStream(RConsole.openOutputStream()));
// pilot.forward();
while (!Button.ENTER.isPressed()) {
System.out.println(s.getColorNumber());
Thread.sleep(300);
}
RConsole.close();
}
private void reportToDevice() {
short sendInt;
if (bdetect.isBox()) {
sendInt = stationIsOccupied;
} else {
sendInt = stationIsNotOccupied;
}
try {
dos.writeInt(sendInt);
} catch (IOException e) {
RConsole.println("[BT] Send failed!");
Delay.msDelay(waitBetweenSends);
closeConnection();
connectToDevice();
reportToDevice();
}
RConsole.println("[BT] Status " + sendInt + " successfully reported!");
}
private void closeConnection() {
try {
dos.close();
btc.close();
} catch (IOException e) {
e.printStackTrace();
}
dos = null;
btc = null;
RConsole.println("[BT] Connection was closed.");
}
private void connectToDevice() {
try {
btc = Bluetooth.waitForConnection();
if (btc == null) {
RConsole.println("[BT] Connection error!");
throw new BluetoothStateException("Connection error!");
}
dos = btc.openDataOutputStream();
} catch (BluetoothStateException e) {
e.printStackTrace();
Delay.msDelay(waitBetweenSends);
connectToDevice();
}
}