My thoughts as an enterprise Java developer.

Saturday, November 01, 2008

Lego Mindstorm NXT program

I temporarily had a Lego Mindstorm NXT and wrote the following program for it using LeJOS.  Enjoy:
import java.util.*;
import lejos.nxt.*;
import lejos.util.*;

public class Dingbot {
public static void main (String[] aArg) throws Exception {
new ButtonWatcherThread(new TimerListener() {
public void timedOut() {
running = !running;
}
}).start();
motorRight.smoothAcceleration(true);
motorLeft.smoothAcceleration(true);
final float circumference = 17.5f;
final int distanceBuffer = 50;
final int rotateDistance = 80;
UltrasonicSensor distSensor = new UltrasonicSensor(SensorPort.S4);
LightSensor litSensor = new LightSensor(SensorPort.S2);
litSensor.setFloodlight(true);

LCD.drawString("Hi",1,1);
int startTime = (int)System.currentTimeMillis();
boolean blinkLight = true;
Random random = new Random();
while(startTime + 60*1000 > (int)System.currentTimeMillis()) {
if(running) {
int cmDist = distSensor.getDistance();
while(running && cmDist > distanceBuffer) {
LCD.drawString("   ",1,2);
LCD.drawInt(cmDist,1,2);
int distance = cmDist - distanceBuffer;
if(distance > 5) {
distance = 3;
}
int degrees = (int)(360 * distance / circumference);
setSpeed();
motorRight.rotate(degrees, true);
motorLeft.rotate(degrees, true);
cmDist = distSensor.getDistance();
litSensor.setFloodlight(blinkLight);
blinkLight = !blinkLight;
LCD.drawInt(litSensor.readNormalizedValue(),1,4);
}
boolean turnPositive = random.nextBoolean();
while(running && cmDist <>
LCD.drawString("   ",1,2);
LCD.drawInt(cmDist,1,2);
setSpeed();
if(turnPositive) {
motorRight.rotate(180);
} else {
motorRight.rotate(-180);
}
cmDist = distSensor.getDistance();
}
} else {
try {
Thread.sleep(500);
} catch(InterruptedException ie) {
//Ignore
}
}
}
}

public static void setSpeed() {
int soundLevel = sound.readValue();
LCD.drawString("   ",1,3);
LCD.drawInt(soundLevel,1,3);
int speed = (100 - soundLevel) * 9;
motorRight.setSpeed(speed);
motorLeft.setSpeed(speed);
}
static Motor motorRight = Motor.B;
static Motor motorLeft = Motor.C;
private static SoundSensor sound = new SoundSensor(SensorPort.S1);
private static boolean running = false;
}

class ButtonWatcherThread extends Thread {
public ButtonWatcherThread(TimerListener tl) {
this.tl = tl;
this.setDaemon(true);
}
public void run() {
wasPressedLast = touchSensor.isPressed();
while(true) {
boolean isPressed = touchSensor.isPressed();
if(wasPressedLast != isPressed) {
if(isPressed) {
tl.timedOut();
}
wasPressedLast = isPressed;
}
try {
Thread.sleep(100);
} catch(InterruptedException ie) {
//Ignore
}
}
}
private TimerListener tl;
private boolean wasPressedLast;
private TouchSensor touchSensor = new TouchSensor(SensorPort.S3);
}

No comments: