public void taskSetUp(int task) {
LCD.clear();
if (menuSelection == 1) {
linePID = new PID(1, 0, 0); // Initialized
rangeFinderPID = new PID(1, 0, 0);
LCD.drawString("Task 1-2", 0, 0);
setPower = 80;
echoTarget = 200;
} else if (menuSelection == 2) {
double a = 1.7;
double b = 1.6;
linePID = new PID(0.33 * a, 2 * a / b, a * b / 3);
rangeFinderPID = new PID(4, 6, .1);
LCD.drawString("Task 3", 0, 0);
setPower = 20;
echoTarget = 890;
// motors.enableReverse();
} else {
LCD.drawString("Invalid Task", 0, 0);
LCD.drawString("Left for", 0, 2);
LCD.drawString("Task 1-2 'curve'", 0, 3);
LCD.drawString("Right for", 0, 5);
LCD.drawString("Task 3 'platoon'", 0, 6);
menuSelection = getTaskNumber();
taskSetUp(menuSelection);
}
LCD.drawString("Press Enter", 0, 1);
LCD.drawString("to Start", 0, 2);
}
public int[] getAngledDistances(DifferentialPilot pilot, int angle, int[] prevVals) {
// const
int sleepTime = 350;
// action info
LCD.clear(0);
LCD.drawString("Finding direction", 0, 0);
// get central dist
prevVals[1] += getFastMeasurement();
LCD.drawInt(prevVals[1], 5, 1);
// rotate, ping, wait a bit
pilot.rotate(angle);
// read left distance
prevVals[0] += getFastMeasurement();
LCD.drawInt(prevVals[0], 0, 1);
// rotate, ping, wait a bit
pilot.rotate(-2 * angle);
// read right distance
prevVals[2] += getFastMeasurement();
LCD.drawInt(prevVals[2], 10, 1);
pilot.rotate(angle);
return prevVals;
}
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();
}
/**
* Gives us the choice between setPower(), or setSpeed(). It allowed us to test which one we
* should use.
*/
public static void test() {
/*
* First, get the values from the user by displaying the instructions on
* the screen. If the user agrees with the parameters set in the menu,
* proceed to the launching. If not, display the menu again.
*/
while (!mainMenu.summarizeLauncher()) {
if (mainMenu.display()) {
launcherPower = mainMenu.askForPower();
} else {
launcherSpeed = mainMenu.askForSpeed();
}
launcherDelay = mainMenu.askForDelay();
}
LCD.clearDisplay();
LCD.drawString(" Shooting ", 0, 0);
if (launcherPower != -1) {
launcher.chainFireAtPower(launcherPower, launcherDelay);
} else {
launcher.chainFireAtSpeed(launcherSpeed, launcherDelay);
}
LCD.clearDisplay();
}
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 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) {
// initialiseer alle equipment
tempMotor = new RCXMotor(MotorPort.A);
lamp = Motor.C;
colorsens = new ColorSensor(SensorPort.S1);
sonar = new UltrasonicSensor(SensorPort.S2);
tempSensor = new RCXTemperatureSensor(SensorPort.S3);
lakes = new Lake[3];
lakes[0] = new Lake(Color.RED);
lakes[1] = new Lake(Color.BLUE);
lakes[2] = new Lake(Color.GREEN);
// zet de robot in de beginstate
current = State.BTINIT;
// opstart-info
LCD.drawString("EndGameRobot", 0, 1);
LCD.drawString("Judith & Mirjam", 0, 2);
Button.waitForAnyPress();
// start de loop of doom
while (!inEndState()) {
execute(current);
}
execute(current); // execute Endstate
}
public static void main(String args[]) throws InterruptedException {
LCD.drawString("Hello World!", 1, 1);
LCD.drawString("Second line", 1, 2);
LCD.drawString("Third line", 1, 3);
Button.ESCAPE.waitForPressAndRelease();
}
public static void main(String[] args) throws Exception {
RCXTemperatureSensor temperature = new RCXTemperatureSensor(SensorPort.S1);
boolean s = true;
while (!Button.ESCAPE.isDown()) {
LCD.clear();
if (s) LCD.drawString("C " + temperature.getCelcius(), 0, 0);
else LCD.drawString("F " + temperature.getFahrenheit(), 0, 0);
s = !s;
LCD.refresh();
Thread.sleep(2000);
}
}
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 Measurement() {
// execute all actions of this state
LCD.drawString("Measurement", 0, 3);
btThread.popElement();
Sound.beep();
Sound.beep();
// lower sensor by RCX motor
tempMotor.setPower(-100);
tempMotor.setPower(0);
for (int i = 0; i < nrcolors; i++) {
if (colorsens.getColorID() == lakes[i].color && !lakes[i].found) // kleuren komen overeen
lakes[i].celsius = tempSensor.getCelcius();
lakes[i].found = true;
return;
}
// raise temp sensor
tempMotor.setPower(100);
btThread.write(500); // sends the message 'action done'
// leg de huidige tijd vast voor alle transitions met een timeoutcondition
long starttime = System.currentTimeMillis();
// when done, wait for a trigger for a transition
boolean transitionTaken = false;
while (!transitionTaken) {
if ((lakes[0].found && lakes[1].found && lakes[2].found)) {
current = State.FINISHED;
transitionTaken = true;
} else if ((true)) {
current = State.WATCH;
transitionTaken = true;
}
}
}
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) {
}
}
}
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);
}
// 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 Finished() {
// execute all actions of this state
LCD.drawString("Finished", 0, 3);
Sound.beep();
Sound.beep();
Sound.beep();
btThread.write(300); // sends the message 'all done'
}
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);
}
/** Shows the calibration parameters, ie offset and scale */
public void showCalibrationSettings() {
LCD.clear();
LCD.drawString("offset range", 3, 0);
LCD.drawString("X", 0, 2);
LCD.drawString("Y", 0, 3);
LCD.drawString("Z", 0, 4);
LCD.drawString("Escape to return", 0, 7);
for (int i = 0; i < 3; i++) {
LCD.drawString(Double.toString(offset[i]), 3, i + 2);
LCD.drawString(" ", 11, i + 2);
LCD.drawString(Double.toString(scale[i]), 10, i + 2);
}
Button.ESCAPE.waitForPressAndRelease();
}
/**
* Asks the user what speed to use for every shoots. This method is used as testing for the
* launcher.
*/
public static void testSpeed() {
launcherDelay = mainMenu.askForDelay();
launcherSpeed = mainMenu.askForSpeed(700);
// Loop if nobody presses the escape button
while (iButton != Button.ID_ESCAPE) {
LCD.clear();
LCD.drawString("S:" + launcherSpeed, 0, 1);
LCD.drawString("D:" + launcherDelay, 0, 2);
LCD.drawString("V:" + Battery.getVoltageMilliVolt(), 0, 3);
launcher.chainFireAtSpeed(launcherSpeed, launcherDelay);
launcherSpeed = mainMenu.askForSpeed(launcherSpeed);
iButton = Button.waitForPress();
}
}
public Follow() {
// TODO Auto-generated constructor stub
ultrasonicSensor = new UltrasonicSensorEcho(49, SensorPort.S4);
display = new FollowDisplay();
LCD.drawString("Left for", 0, 0);
LCD.drawString("Task 1-2 'curve'", 0, 1);
LCD.drawString("Right for", 0, 3);
LCD.drawString("Task 3 'platoon'", 0, 4);
IRArray = new IRSensorArray(SensorPort.S1, SensorPort.S2, SensorPort.S3);
menuSelection = getTaskNumber();
taskSetUp(menuSelection);
motors = new MotorController(MotorPort.A, MotorPort.B);
SensorPort.S4.setSensorPinMode(SensorPort.SP_DIGI0, SensorPort.SP_MODE_INPUT);
SensorPort.S4.setSensorPinMode(SensorPort.SP_DIGI1, SensorPort.SP_MODE_OUTPUT);
display.follow = this;
new Thread(this).start();
}
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[] arguments) {
Motor.A.setSpeed(200);
Motor.B.setSpeed(200);
Motor.A.forward();
Motor.B.forward();
final LightSensor sensor = new LightSensor(SensorPort.S1);
final Object blocker = new Object();
while (true) {
if (sensor.readNormalizedValue() >= 512) {
LCD.bitBlt(
null,
LCD.SCREEN_WIDTH,
LCD.SCREEN_HEIGHT,
0,
0,
0,
0,
LCD.SCREEN_WIDTH,
LCD.SCREEN_HEIGHT,
LCD.ROP_CLEAR);
} else {
LCD.bitBlt(
null,
LCD.SCREEN_WIDTH,
LCD.SCREEN_HEIGHT,
0,
0,
0,
0,
LCD.SCREEN_WIDTH,
LCD.SCREEN_HEIGHT,
LCD.ROP_SET);
}
synchronized (blocker) {
try {
blocker.wait(100);
} catch (InterruptedException e) {
}
}
}
}
public static void main(String[] args) throws Exception {
LCD.drawString("waiting", 0, 0);
USBConnection conn = USB.waitForConnection();
DataOutputStream dOut = conn.openDataOutputStream();
DataInputStream dIn = conn.openDataInputStream();
while (true) {
int b;
try {
b = dIn.readInt();
} catch (EOFException e) {
break;
}
dOut.writeInt(-b);
dOut.flush();
LCD.drawInt(b, 8, 0, 1);
}
dOut.close();
dIn.close();
conn.close();
}
public void start(iStopCondition stopCond, boolean forward) {
final int power = 300;
int sensorVal, powerDiff, whiteDiff = 0;
pidController = new PIDController(sensor.blackWhiteThreshold, 0);
pidController.setPIDParam(PIDController.PID_KP, 1.8f);
pidController.setPIDParam(PIDController.PID_KD, 8f);
// LCD.clear();
LCD.drawString("Light: ", 0, 1);
LCD.drawString("pDiff: ", 0, 2);
while (!stopCond.stopLoop()) {
// whiteDiff = (sensorRef.wasWhite() ?
// sensorRef.getChachedLightValue() - whiteRef : whiteDiff);
sensorVal = sensor.light() - whiteDiff / 2;
powerDiff = pidController.doPID(sensorVal);
if (forward) {
pilot.forward(power + powerDiff, power - powerDiff);
} else {
pilot.backward(power - powerDiff, power + powerDiff);
}
LCD.drawInt(sensorVal, 4, 10, 1);
LCD.drawInt(powerDiff, 4, 10, 2);
LCD.refresh();
Thread.yield();
}
pilot.stop();
LCD.clear();
}
// a method for every state
public static void Btinit() {
// execute all actions of this state
LCD.drawString("Btinit", 0, 3);
LCD.drawString("Waiting...", 0, 0);
LCD.refresh();
BTConnection btc = Bluetooth.waitForConnection();
LCD.clear();
LCD.drawString("Connected", 0, 0);
LCD.refresh();
DataInputStream dis = btc.openDataInputStream();
DataOutputStream dos = btc.openDataOutputStream();
// BTfunctionality btThread = new BTfunctionality("SlaveReader",dis,dos);
btThread = new BTfunctionality("SlaveReader", dis, dos);
btThread.start();
// leg de huidige tijd vast voor alle transitions met een timeoutcondition
long starttime = System.currentTimeMillis();
// when done, wait for a trigger for a transition
boolean transitionTaken = false;
while (!transitionTaken) {
if ((true)) {
current = State.WATCH;
transitionTaken = true;
}
}
}
/**
* Displays the raw value of axis indicated by index. A low pass filter is applied on the raw
* values to make reading easier.
*
* @param index Indicates the axis (x=0,y=1,z=2).
*/
private void showLowPass(int index) {
float alpha = 0.95f;
float lowpassed = 0;
while (!Button.ENTER.isDown()) {
accel.fetchRawAccel(raw);
lowpassed = alpha * raw[index] + (1.0f - alpha) * lowpassed;
LCD.drawString(Double.toString(lowpassed), 0, 3);
try {
Thread.sleep(150);
} catch (InterruptedException ex) {
}
}
while (Button.ENTER.isDown()) ;
}
public void run() {
try {
// TODO: clean closing of connection
for (; ; ) {
BTConnection btc = Bluetooth.waitForConnection();
DataInputStream dis = btc.openDataInputStream();
// Read Input-Number from BTSend
int n = dis.readInt();
switch (n) {
case 0:
// up
swp.forward();
break;
case 1:
// down
swp.backward();
break;
case 2:
// left
swp.arc(-1, 60, true);
break;
case 3:
// right
swp.arc(1, 60, true);
break;
case 4:
// Stop
swp.stop();
break;
default:
// do nothing
break;
}
LCD.clear();
dis.close();
btc.close();
}
} catch (Exception ioe) {
// LCD.drawString(ioe.getMessage(), 2, 0);
}
}
/**
* 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) {
}
}
@Override
public void run() {
while (ca.isKeepOn()) {
if (!cmdList.isEmpty()) {
Command cmd = removeCommand();
cmd.execute();
}
try {
Thread.sleep(100);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
LCD.drawString("exit CommandExecutor", 0, 0);
}
public static void Sonarflag() {
// execute all actions of this state
LCD.drawString("Sonarflag", 0, 3);
Sound.beep();
btThread.write(sonar.getDistance()); // sends the data from the sonar
// leg de huidige tijd vast voor alle transitions met een timeoutcondition
long starttime = System.currentTimeMillis();
// when done, wait for a trigger for a transition
boolean transitionTaken = false;
while (!transitionTaken) {
if ((true)) {
current = State.WATCH;
transitionTaken = true;
}
}
}
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) {
}
}
}
