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 synchronized void run() {
if ((line >= 0) && (line <= 7)) {
LCD.drawString(" ", 0, getLine());
LCD.drawString(getAString(), 0, getLine());
LCD.refresh();
}
}
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();
}
}
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 static void main(String[] args) throws Exception {
String[] connectionStrings = new String[] {"Bluetooth", "USB", "RS485"};
TextMenu connectionMenu = new TextMenu(connectionStrings, 0, "Connection");
NXTCommConnector[] connectors = {
Bluetooth.getConnector(), USB.getConnector(), RS485.getConnector()
};
int connectionType = connectionMenu.select();
LCD.clear();
LCD.clear();
LCD.drawString("Type: " + connectionStrings[connectionType], 0, 0);
LCD.drawString("Running...", 0, 1);
Responder resp = new Responder(connectors[connectionType]);
resp.start();
resp.join();
LCD.drawString("Closing... ", 0, 1);
}
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) {
}
}
}
// The recognize() method returns true if the object is a blue block
public boolean recognize() {
// Boolean that determines whether or not to start detecting what kind of object is in front
boolean isRecognizing = true;
while (isRecognizing) {
// The robot will drive slowly if the light reads under 300, which means an object is closeby
while (color.getNormalizedLightValue() < 300) {
robot.setForwardSpeed(2);
}
// When the robot is close enough to the object, it will stop then turn on its
// blue light which gives two noticeable different light values for a wooden and blue
// block
robot.setForwardSpeed(0);
color.setFloodlight(Color.BLUE);
// If the light value of blue reads over 250, it is a blue block
if (color.getNormalizedLightValue() >= 250) {
Sound.beep();
LCD.drawString("Blue styrofoam block", 3, 5);
// Turn the default light back on after analyzing an object
color.setFloodlight(true);
return true;
} else { // If it is not a blue block, it is a wooden block
Sound.buzz();
LCD.drawString("Wooden block", 3, 5);
// Turn the default light back on after analyzing an object
color.setFloodlight(true);
}
// Set the boolean to false to break out of the while loop and stop analyzing the object
isRecognizing = false;
}
// Default return value. It should never get here but JAVA requires a return type
return false;
}
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() {
int count = 0;
while (true) {
LCD.drawString("" + Integer.toString(count++), 6, 0);
count %= 1000;
try {
Thread.sleep(100);
} catch (InterruptedException e) {
}
}
}
public static void main(String args[]) {
int sensorID, sensorValue;
RCXLNPPort port = null;
try {
// if(args.length!=1)
// throw new Exception("first argument must be tower port (USB,COM1 etc)");
port = new RCXLNPPort();
DataOutputStream out = new DataOutputStream(port.getOutputStream());
while (true) {
sensorID = port.getInputStream().read();
sensorValue = Sensor.readSensorValue(sensorID, 0);
LCD.showNumber(sensorValue);
out.writeShort(sensorValue);
out.flush();
}
} catch (IOException ioE) {
LCD.showNumber(1111);
} finally {
if (port != null) port.close();
}
}
/**
* 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) {
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) {
// grid values for mapping
columns = 4;
numOfRowCells = 5;
cellSize = 30;
// connect to other classes
mapObj = new MapSystem(cellSize, columns, numOfRowCells);
movObj = new Movement(cellSize, columns, numOfRowCells);
// connect to RConsole
btObj.startBtConn();
System.out.println("Press Any Button");
Button.waitForAnyPress();
LCD.clear();
// for each column
for (loop1 = 0; loop1 < columns; loop1++) {
// for each cell within the column
for (loop2 = 0; loop2 < numOfRowCells; loop2++) {
movRow(); // scan and move method
}
if (loop1 == (columns - 1)) {
break;
}
if (loop1 % 2 == 0) { // if the robot moves along the Y axis positively
movCol(true); // turn right into the new column
} else {
movCol(false); // turn left into the new column
}
}
String[] output = mapObj.getMap(columns, numOfRowCells);
Button.waitForAnyPress();
btObj.stringToRCon(output[0]);
btObj.stringToRCon(output[1]);
btObj.stringToRCon(output[2]);
}
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();
}
}
}
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) {
}
}
}
/** Internal method used to clear some rows in User Interface */
private static void clearRows() {
LCD.drawString(" ", 0, 3);
LCD.drawString(" ", 0, 4);
LCD.drawString(" ", 0, 5);
}
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();
}
}
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() {
// infinite task
while (true) {
// retrieve decoded opcode info
synchronized (this) {
newDyn = DEObj.getNxtDyn();
newCmd = DEObj.getNxtCmd();
newPos = DEObj.getNxtPos();
curPos = centreMotor.getTachoCount();
DEObj.setCurSteerPos(curPos);
}
;
// steer to newPos, if conditions apply
if ((newCmd != DataExchange.EXIT_CMD) && (newCmd != DataExchange.NULL_CMD)) {
if (oldDyn != newDyn) {
oldDyn = newDyn;
switch (newCmd) {
case DataExchange.STOP_CMD:
isNxtStop = true;
centreMotor.stop();
break;
case DataExchange.CAL_CMD:
centreMotor.rotate(getDeltaPos(newPos, curPos));
LCD.clear(6);
LCD.drawString("CMD:: CALIBRATE", 0, 6);
break;
case DataExchange.START_CMD:
isNxtStop = false;
if (isNxtCalib) {
// calibration is done, the current TachoCount
// position becomes the reference, i.e. "0 deg".
isNxtCalib = false;
centreMotor.resetTachoCount();
curPos = centreMotor.getTachoCount();
}
break;
case DataExchange.RELEASE_CMD:
// when user releases the mouse drag, the vehicle attempts to recover a straight
// drive.
// It simulates what happens naturally in a real car when the driver gets his hands
// off
// the steering upon exiting from a curve.
centreMotor.rotateTo(0);
break;
case DataExchange.FRWD_CMD:
case DataExchange.BKWD_CMD:
if (!isNxtStop) {
centreMotor.rotate(getDeltaPos(newPos, curPos));
}
break;
default:
break;
}
; // switch case
} // if (old != new)
else {
centreMotor.flt();
}
try {
// thread going to sleep
ManageNxtSteering.sleep(50);
} catch (InterruptedException e) {
e.printStackTrace();
LCD.drawString("EX-6:", 0, 5);
LCD.refresh();
}
} // if (new != EXIT_CMD)
else {
if (newCmd == DataExchange.EXIT_CMD) {
centreMotor.stop(); /* stop motor */
break; /* exit while loop */
}
}
} // while loop
DEObj.setNXTdone(true);
}
public void action() {
LCD.clear();
LCD.drawInt(sonar.getDistance(), 0, 0);
LCD.refresh();
}
