public static void main(String[] args) throws InterruptedException {
// Creates an area of motors
Motor[] m = {Motor.A, Motor.B};
// Spins them forward for 3 seconds
for (int i = 0; i < 2; i++) {
m[i].forward();
}
LCD.drawString("FORWARD", 0, 0);
Thread.sleep(3000);
// Spins them backward for 3 seconds
for (int i = 0; i < 2; i++) {
m[i].backward();
}
LCD.drawString("BACKWARD", 0, 1);
Thread.sleep(3000);
// Spins them forward again for 3 seconds
for (int i = 0; i < 2; i++) {
m[i].reverseDirection();
}
LCD.drawString("FORWARD", 0, 2);
Thread.sleep(3000);
// Stops the motors
for (int i = 0; i < 2; i++) {
m[i].stop();
}
}
static void init() throws InterruptedException {
Sound.beep();
try {
File f = new File("CalibrationData.dat");
FileInputStream is = new FileInputStream(f);
colors = RescueColors.readObject(is);
is.close();
LCD.drawString("File read", 0, 0);
LCD.drawString("sucessfully", 0, 0);
} catch (IOException e) {
LCD.drawString("Error reading", 0, 0);
LCD.drawString("file", 0, 1);
Thread.sleep(1000);
System.exit(-1);
}
colors.printToLCD();
robot = new RescueRobot(colors);
Button.ENTER.waitForPressAndRelease();
logger = new MovementLogger(robot);
Thread t = new Thread(logger);
t.setDaemon(true);
t.start();
}
public static void main(String[] aArg) throws Exception {
String left = "Turn left ";
String right = "Turn right";
LightSensor light = new LightSensor(SensorPort.S3);
final int blackWhiteThreshold = 45;
DataLogger dl = new DataLogger("Memory10MS_C.txt");
int lightValue;
Runtime rt = Runtime.getRuntime();
final int forward = 1;
final int stop = 3;
final int flt = 4;
final int power = 80;
// Use the light sensor as a reflection sensor
light.setFloodlight(true);
LCD.drawString("Light %: ", 0, 0);
// Show light percent until LEFT is pressed
LCD.drawString("Press LEFT", 0, 2);
LCD.drawString("to start", 0, 3);
while (!Button.LEFT.isDown()) {
LCD.drawInt(light.readValue(), 3, 9, 0);
}
// Follow line until ESCAPE is pressed
LCD.drawString("Press ESCAPE", 0, 2);
LCD.drawString("to stop ", 0, 3);
dl.start();
while (!Button.ESCAPE.isDown()) {
lightValue = light.readValue();
if (lightValue > blackWhiteThreshold) {
// On white, turn right
LCD.drawString(right, 0, 1);
MotorPort.B.controlMotor(0, stop);
MotorPort.C.controlMotor(power, forward);
} else {
// On black, turn left
LCD.drawString(left, 0, 1);
MotorPort.B.controlMotor(power, forward);
MotorPort.C.controlMotor(0, stop);
}
LCD.drawInt(lightValue, 3, 9, 0);
dl.writeSample(rt.freeMemory());
Thread.sleep(10);
}
// Stop car gently with free wheel drive
MotorPort.B.controlMotor(0, flt);
MotorPort.C.controlMotor(0, flt);
LCD.clear();
dl.close();
LCD.drawString("Program stopped", 0, 0);
Thread.sleep(1000);
}
public void run() {
connect();
try {
collectMessage();
} catch (InterruptedException e) {
Thread msgInterruptDisplay = new ScreenWriter("Msg Col Interupt", 7);
msgInterruptDisplay.start();
}
try {
Thread.sleep(100);
} catch (InterruptedException e) {
}
}
public static void main(String... args) throws InterruptedException {
init();
long lastVictimTime = 0;
while (true) {
RawColor color = colors.getSensorColor(robot.colorSensor);
if ((color == colors.silver || color == colors.green)
&& System.currentTimeMillis() - lastVictimTime > 2500) {
synchronized (robot.pilot) {
robot.pilot.stop();
robot.showVictimFound();
lastVictimTime = System.currentTimeMillis();
robot.pilot.travel(5);
findline();
}
} else if (color == colors.white || color == colors.black) {
synchronized (robot.pilot) {
int[] sensors = robot.lineSensor.getSensors();
if (sensors != null) {
int leftspeed = calcLeftSpeed(sensors);
int rightspeed = calcRightSpeed(sensors);
robot.pilot.setLeftSpeed(leftspeed);
robot.pilot.setRightSpeed(rightspeed);
}
}
}
Thread.sleep(50);
}
}
public void run() {
int count = 0;
while (true) {
LCD.drawString("" + Integer.toString(count++), 6, 0);
count %= 1000;
try {
Thread.sleep(100);
} catch (InterruptedException e) {
}
}
}
private static void collectMessage() throws InterruptedException {
boolean atend = false;
int N = 0;
while (atend == false) {
N = N + 1; // % 100;
Movement.setCommandCount(N);
LCD.drawString("Recv:" + Integer.toString(N), 2, 2);
try {
// Bluetooth.getConnectionStatus();
int message = inputStream.readInt();
LCD.drawString("Rcvd:" + Integer.toString(N), 2, 3);
if (message >= (1 << 26)) {
LCD.drawString("end" + Integer.toString(N), 12, 2);
atend = true;
// Thread atendDisplay = new ScreenWriter(Integer.toString(message),7);
LCD.drawString(Integer.toString(message), 0, 7);
// atendDisplay.start();
// System.exit();
LCD.drawString("stopped" + message, 0, 2);
} else if (message < (1 << 26)) {
// Thread newMessageDisplay = new ScreenWriter(Integer.toString(message),6);
// LCD.drawString("display"+Integer.toString(N), 6, 0);
// newMessageDisplay.start();
LCD.drawString(" ", 5, 6);
LCD.drawString("Msg:" + Integer.toString(message), 0, 6);
// LCD.drawString("decode:"+Integer.toString(N), 6, 1);
parseMessage(message);
// LCD.drawString("decoded:"+Integer.toString(N), 6, 0);
}
// inputStream.close();
// inputStream = connection.openDataInputStream();
} catch (IOException e) {
Thread errorConnection = new ScreenWriter("Error - connect back up", 7);
errorConnection.start();
// connection = Bluetooth.waitForConnection();
Thread connectedDisplay = new ScreenWriter("Connection Opened", 7);
connectedDisplay.start();
}
}
}
/**
* Final Message
*
* @param seconds
*/
private static void credits(int seconds) {
LCD.clear();
LCD.drawString("LEGO Mindstorms", 0, 1);
LCD.drawString("NXT Robots ", 0, 2);
LCD.drawString("run better with", 0, 3);
LCD.drawString("Java leJOS", 0, 4);
LCD.drawString("www.lejos.org", 0, 6);
LCD.refresh();
try {
Thread.sleep(seconds * 1000);
} catch (Exception e) {
}
}
public static void main(String[] args) {
LCD.drawString(appName, 0, 0);
LCD.drawString("#################", 0, 2);
LCD.drawString("#################", 0, 6);
msc = new MSC(SensorPort.S1);
// Set to initial angle
msc.servo1.setAngle(90);
int angle = 0;
int pulse = 0;
int NXTServoBattery = 0;
while (!Button.ESCAPE.isPressed()) {
NXTServoBattery = msc.getBattery();
if (Button.LEFT.isPressed()) {
angle = 0;
msc.servo1.setAngle(angle);
}
if (Button.ENTER.isPressed()) {
angle = 90;
msc.servo1.setAngle(angle);
}
if (Button.RIGHT.isPressed()) {
angle = 180;
msc.servo1.setAngle(angle);
}
clearRows();
LCD.drawString("Battery: " + NXTServoBattery, 0, 3);
LCD.drawString("Pulse: " + msc.servo1.getPulse(), 0, 4);
LCD.drawString("Angle: " + msc.servo1.getAngle(), 0, 5);
LCD.refresh();
}
// Set to initial angle
msc.servo1.setAngle(90);
LCD.drawString("Test finished", 0, 7);
LCD.refresh();
try {
Thread.sleep(1000);
} catch (Exception e) {
}
credits(3);
System.exit(0);
}
public void run() {
motor_left.resetTachoCount();
motor_left.regulateSpeed(true);
Movement.motor_left.smoothAcceleration(true);
int previousCommandCount = -1;
while (true) {
if (Movement.getCommandCount() == previousCommandCount) {
try {
Thread.sleep(10);
} catch (InterruptedException e) {
}
continue;
}
previousCommandCount = Movement.getCommandCount();
setToAngle(ControlCentre.getTargetSteeringAngleLeft());
int new_angle = getToAngle();
if (new_angle < 10) LCD.drawString(" ", 8, 1);
else if (new_angle < 100) LCD.drawString(" ", 9, 1);
LCD.drawString(Integer.toString(new_angle), 7, 1);
LCD.drawString("R", 11, 1);
int cur_angle = getCurrentSteeringAngle();
double delta = new_angle - cur_angle;
final double C = Movement.rotConstant;
double turn_angle = 0;
if (Math.abs(delta) < thresholdAngle / 2.0) {
continue;
} else if (Math.abs(delta) >= thresholdAngle / 2.0 && Math.abs(delta) < thresholdAngle) {
delta = thresholdAngle * delta / Math.abs(delta);
}
setCurrentSteeringAngle((int) (cur_angle + delta) % 360);
if (delta != 0 && Math.abs(delta) < 180) {
turn_angle = C * delta;
} else if (delta >= 180 && delta < 360) {
turn_angle = -C * (360 - delta);
} else if (delta <= -180) {
turn_angle = C * (360 + delta);
} else {
/* No turning needed */
continue;
}
motor_left.rotate((int) Math.round(turn_angle));
}
}
public void run() {
while (true) {
boolean kick = ControlCentre.getKickState();
if (kick) {
LCD.drawString("K,", 0, 1);
Movement.motor_kick.setSpeed(900);
Movement.motor_kick.rotate((-120 * (5 / 3)));
Movement.motor_kick.rotate((120 * (5 / 3)));
} else {
LCD.drawString("_,", 0, 1);
}
try {
Thread.sleep(100);
} catch (InterruptedException e) {
}
}
}
// Aims to establish a conection over Bluetooth
private static void connect() {
Thread tryingDisplay = new ScreenWriter("Trying to connect", 7);
tryingDisplay.start();
// Wait until connected
connection = Bluetooth.waitForConnection();
Thread connectedDisplay = new ScreenWriter("Connected", 7);
connectedDisplay.start();
inputStream = connection.openDataInputStream();
outputStream = connection.openDataOutputStream();
Thread openConnDisplay = new ScreenWriter("Connection Opened", 7);
openConnDisplay.start();
}
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;
}
public static void main(String[] args) {
HTSensorMux3 sm1 = new HTSensorMux3(SensorPort.S1);
// LCD.drawString("" + sm1.getProductID(), 0,2);
// LCD.drawString("" + sm1.getVersion(), 0,3);
// LCD.drawString("" + sm1.getSensorType(), 0,4);
// LCD.refresh();
sm1.configurateMUX2();
int distance = 0;
while (!Button.ESCAPE.isPressed()) {
distance = sm1.getDistance();
LCD.drawString("" + distance, 0, 0);
try {
Thread.sleep(100);
} catch (Exception e) {
}
}
}
public static void main(String[] args) throws Exception {
String connected = "Connected";
String waiting = "Waiting...";
String closing = "Closing...";
while (true) {
LCD.drawString(waiting, 0, 0);
LCD.refresh();
// BTConnection btc = Bluetooth.waitForConnection();
BTConnection btc = Bluetooth.waitForConnection(0, NXTConnection.RAW);
LCD.clear();
LCD.drawString(connected, 0, 0);
LCD.refresh();
DataInputStream dis = btc.openDataInputStream();
DataOutputStream dos = btc.openDataOutputStream();
for (int i = 0; i < 100; i++) {
int n = dis.readInt();
LCD.drawInt(n, 7, 0, 1);
LCD.refresh();
dos.writeInt(-n);
dos.flush();
}
dis.close();
dos.close();
Thread.sleep(100); // wait for data to drain
LCD.clear();
LCD.drawString(closing, 0, 0);
LCD.refresh();
btc.close();
LCD.clear();
}
}
/**
* This recursive method is responsible for finding the blue styrofoam block and returning it to
* the green/red zone. The search and detect process will begin when the robot has reached the
* green/red zone.
*
* @param x The x position of where the robot will start the search for the blue styrofoam block
* @param y The y position of where the robot will start search for the blue styrofoam block
*/
public void doSearchAndDetect(double x, double y) { // Searching inside the green/red zone
if (firstTry) {
odox = odometer.getX();
odoy = odometer.getY();
odotheta = odometer.getTheta();
dashboard.turnLeft(40);
try {
Thread.sleep(750);
} catch (Exception e) {
}
} else if (Math.abs(odox - odometer.getX()) > 15 && Math.abs(odoy - odometer.getY()) > 15) {
odox = odometer.getX();
odoy = odometer.getY();
odotheta = odometer.getTheta();
dashboard.turnLeft(40);
try {
Thread.sleep(750);
} catch (Exception e) {
}
}
firstTry = false;
// Initializing the heading angle when a robot detects something 35 cm away
detectionAngle = odometer.getTheta();
Point greenZone = new Point((int) x, (int) y); // Remembering green zone
dashboard.turnLeft(70); // Beginning the 360 degrees scan
boolean clawIsDown = false;
// The boolean found never becomes true so the robot will keep spinning until
// it finds something then moves to a different zone or it will complete 1 full
// revolution from its starting angle then move to a different zone.
while (!found) {
// Condition for the robot to move to a different area to search since it has
// completed one full revolution
if (Math.abs(odox - odometer.getX()) < 10
&& Math.abs(odoy - odometer.getY()) < 10
&& Math.abs(odotheta - odometer.getTheta())
< 5) { // Case where no objects in search radius
if (directionCounter
== 1) { // First time around, go to a point 15 cm south west of the green zone bottom
// left corner
directionCounter++;
navigation.travelTo(greenZone.getX() - 15, greenZone.getY() - 15, false);
} else if (directionCounter
== 2) { // Second time around, go below the middle of the green zone
directionCounter++;
navigation.travelTo(greenZone.getX() + 30, greenZone.getY() - 15, false);
} else if (directionCounter == 3) { // Third time, go north easy of of the green zone
directionCounter++;
navigation.travelTo(greenZone.getX() + 30, greenZone.getY() + 45, false);
} else if (directionCounter
== 4) { // Last time around, go to the middle of the playing field
directionCounter = 1;
navigation.travelTo(180, 180, false);
}
// After traveling to the new scan zone, perform the search and scan
doSearchAndDetect(greenZone.getX(), greenZone.getY());
} else {
// If the ultrasonic sensor senses something within 35 centimeters, go to the object
if (usLow.getFilteredDistance() <= 35
&& Math.abs(odometer.getTheta() - badHeading) > 30) { // Range to go check out object
detectionAngle = odometer.getTheta(); // Saving heading angle when the object is detected
blockAngle = odometer.getTheta();
previousDistance = usLow.getFilteredDistance();
navigation.turnTo(
detectionAngle
- 30.0); // Turning to past where object was detected so it can analyze the full
// block
Stopwatch stopwatch = new Stopwatch();
// Scanning whole object for 3 seconds to find shortest point of the object to robot
while (stopwatch.elapsed() < (3 * Math.pow(10, 3))) {
dashboard.turnRight(
70); // Turning to the right to analyze object since it overturned initially
if (usLow.getFilteredDistance() < previousDistance) {
dashboard.stop();
previousDistance = usLow.getFilteredDistance();
blockAngle = odometer.getTheta();
}
}
dashboard.stop();
navigation.turnTo(blockAngle); // Turn to block
// Remembering tacho count in case object is a wooden block
// so it will back up to exactly the same spot
originalTacho = dashboard.getRightTacho();
Stopwatch watch = new Stopwatch();
while (usLow.getFilteredDistance() >= 10) {
// If robot hasn't reached object in 8 seconds, most likely nothing there and false
// positive
if (watch.elapsed() > 9 * Math.pow(10, 3)) {
badHeading = odometer.getTheta();
newTacho = dashboard.getRightTacho();
dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
dashboard.stop();
doSearchAndDetect(greenZone.getX(), greenZone.getY());
}
// Travel to the object by going forward
dashboard.goForward(200);
}
// Making a check for a wooden block by using the top ultrasonic sensor
if (usHigh.getFilteredDistance()
< 25) { // Brown Block so back up distance traveled to the block
dashboard.stop();
badHeading =
odometer
.getTheta(); // Saving the angle of the wooden block so it doesn't go back when
// it sees it again
newTacho = dashboard.getRightTacho();
dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
dashboard.stop();
doSearchAndDetect(greenZone.getX(), greenZone.getY());
} else { // Go forward a bit more to get closer to the styrofoam block
for (int i = 0; i < 200; i++) {
dashboard.goForward(70);
}
}
// 2nd check for brown block to make sure when closer to object
if (usHigh.getFilteredDistance() < 25) {
dashboard.stop();
badHeading = odometer.getTheta();
newTacho = dashboard.getRightTacho();
dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
dashboard.stop();
doSearchAndDetect(greenZone.getX(), greenZone.getY());
} else { // Return to dropzone then drop off and re-do scan
newTacho = dashboard.getRightTacho();
// If doing search but the block is in the green/red zone, don't pick it up again
if ((odometer.getX() > greenZone.getX() && odometer.getY() > greenZone.getY())
&& (odometer.getX() < greenZone.getX() + 30
&& odometer.getY() < greenZone.getY() + 30)) {
badHeading = odometer.getTheta();
dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
dashboard.stop();
doSearchAndDetect(greenZone.getX(), greenZone.getY());
}
dashboard.stop();
// Reached a good distance from the styrofoam block so back up
// and get ready for the juggle.
Stopwatch secondWatch = new Stopwatch();
while (secondWatch.elapsed() < 3 * Math.pow(10, 3)) {
dashboard.goBackward(100);
}
dashboard.stop();
if (!clawIsDown) {
// Arms are currently up so drop them since the block is
// detected and close by
lifter.dropClaw();
clawIsDown = true;
}
// Driving forward until the block is within 7 cm
while (usLow.getFilteredDistance() >= 7) {
dashboard.goForward(100);
}
juggle(); // Juggle to reposition the block so it is easier to pick
dashboard.stop();
// Drive forward 8 cm so the block will be picked up perfectly
dashboard.goForward(8, false);
// Remember where the block was picked up so the robot
// will return here after dropping off in the designated zone
previousPickUp.setLocation(odometer.getX(), odometer.getY());
dashboard.stop();
lifter.lift(); // Picking up the block
isCarrying = true;
// Driving back to the green zone (a bit further to account) for the arms
// and at this distance, the block is dropped off in the middle of the zone
navigation.travelTo(greenZone.getX() + 45, greenZone.getY() + 45, false);
// If it is the nth*2 time, robot will stack.
if (!firstIteration) {
lifter.stackLift();
}
navigation.turnTo(180); // Turning so the block will end up in the middle of the zone
dashboard.goForward(
2,
false); // Going forward 2 cm so even closer to block (used for stacking purposes)
lifter.dropBlock();
Stopwatch secondStop = new Stopwatch();
// After dropping it off, back up then lift arms
while (secondStop.elapsed() < 3.5 * Math.pow(10, 3)) {
dashboard.goBackward(100);
}
dashboard.stop();
lifter.postLift();
firstIteration = !firstIteration; // Only making stacks of 2 so reverse boolean
navigation.travelTo(
greenZone.getX() - 15,
greenZone.getY(),
false); // Travel to side of green zone so blocks are not touched when navigating
// out of the zone
navigation.travelTo(
previousPickUp.getX(),
previousPickUp.getY(),
false); // Return to previous position of where styrofoam block was found
doSearchAndDetect(
greenZone.getX(), greenZone.getY()); // Do search and detect at the new position
}
}
}
}
}
public void run() {
Multiplexor chip = new Multiplexor(SensorPort.S4);
while (true) {
int targetLeft = ControlCentre.getTargetDriveLeftVal();
LCD.drawString(Integer.toString(targetLeft) + ",", 2, 1);
switch (targetLeft) {
case 0:
chip.setMotors(0, 0, 0);
break;
case 4:
chip.setMotors(0, 0, 0);
break;
case 1:
chip.setMotors(1, 1, 0);
break;
case 2:
chip.setMotors(1, 2, 0);
break;
case 3:
chip.setMotors(1, 3, 0);
break;
case 5:
chip.setMotors(-1, 1, 0);
break;
case 6:
chip.setMotors(-1, 2, 0);
break;
case 7:
chip.setMotors(-1, 3, 0);
break;
}
int targetRight = ControlCentre.getTargetDriveRightVal();
LCD.drawString(Integer.toString(targetRight) + " L", 4, 1);
switch (targetRight) {
case 0:
chip.setMotors(0, 0, 1);
break;
case 4:
chip.setMotors(0, 0, 1);
break;
case 1:
chip.setMotors(1, 1, 1);
break;
case 2:
chip.setMotors(1, 2, 1);
break;
case 3:
chip.setMotors(1, 3, 1);
break;
case 5:
chip.setMotors(-1, 1, 1);
break;
case 6:
chip.setMotors(-1, 2, 1);
break;
case 7:
chip.setMotors(-1, 3, 1);
break;
}
try {
Thread.sleep(10);
} catch (InterruptedException e) {
}
}
}
public static void main(String[] args) throws InterruptedException {
Thread mainCommunicator = new Communicator();
Thread kickThread = new KickThread();
Thread driveThread = new DriveThread();
Thread steeringLeftThread = new SteeringLeftThread();
Thread steeringRightThread = new SteeringRightThread();
Thread counterThread = new CounterThread();
mainCommunicator.start();
kickThread.start();
driveThread.start();
steeringLeftThread.start();
steeringRightThread.start();
counterThread.start();
}