/**
* * Move the robot and update the distribution with the action model
*
* @param distance
* @param _heading
* @param _sensorModel
*/
private void move(
float distance, Heading _heading, ActionModel _actionModel, SensorModel _sensorModel) {
// move robot
m_robot.translate(m_translationAmount);
// now update estimate of position using the action model
m_distribution = _actionModel.updateAfterMove(m_distribution, _heading);
// if visualising, update the shown distribution
if (m_mapVis != null) {
m_mapVis.setDistribution(m_distribution);
}
// A short delay so we can see what's going on
Delay.msDelay(1000);
m_distribution =
_sensorModel.updateAfterSensing(m_distribution, _heading, m_robot.getRangeValues());
// if visualising, update the shown distribution
if (m_mapVis != null) {
m_mapVis.setDistribution(m_distribution);
}
// A short delay so we can see what's going on
Delay.msDelay(1000);
}
public void run() {
while (true) {
connectToDevice();
reportToDevice();
closeConnection();
Delay.msDelay(waitBetweenSends);
}
}
// 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);
}
}
// 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);
}
/** Run. */
public void run() {
Motor.A.setSpeed(90);
Motor.C.setSpeed(90);
while (true) {
moveForward();
waitForReading();
Motor.A.backward();
Motor.C.backward();
Delay.msDelay(1000);
rotation(TURN_90_DEGREES, TURN_LEFT);
}
}
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();
}
}
/**
* 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();
}
public static boolean findline() {
TachoPilot p = robot.pilot;
int[] angles = new int[] {30, -30, 60, -60, 90, -90, 0};
int lastangle = 0;
for (int angle : angles) {
p.rotate((lastangle - angle) * 4, true);
while (p.isMoving()) {
if (linefound()) {
Sound.beepSequence();
p.stop();
return true;
}
Thread.yield();
}
p.stop();
lastangle = angle;
Delay.msDelay(250);
}
return false;
}
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 int waitForReading() {
int lightLevelReading;
int distanceToObject;
do {
try {
Thread.sleep(100);
} catch (InterruptedException ex) {
}
lightLevelReading = lightSensorDown.readValue();
} while (lightLevelReading > BLACK_WHITE_THRESHOLD);
distanceToObject = ultrasonicSensor.getDistance();
if (distanceToObject < 20) {
Motor.A.setSpeed(1000);
Motor.C.setSpeed(1000);
pilot.travel(300);
Delay.msDelay(10000);
}
return (lightLevelReading);
}
