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();
}
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 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;
}
public static void main(String[] args) throws Exception {
ColorHTSensor cmps = new ColorHTSensor(SensorPort.S1);
String color = "Color";
String r = "R";
String g = "G";
String b = "B";
String[] colorNames = {
"Red",
"Green",
"Blue",
"Yellow",
"Magenta",
"Orange",
"White",
"Black",
"Pink",
"Gray",
"Light gray",
"Dark Gray",
"Cyan"
};
while (!Button.ESCAPE.isDown()) {
LCD.clear();
LCD.drawString(cmps.getVendorID(), 0, 0);
LCD.drawString(cmps.getProductID(), 0, 1);
LCD.drawString(cmps.getVersion(), 9, 1);
LCD.drawString(color, 0, 3);
LCD.drawInt(cmps.getColorID(), 7, 3);
LCD.drawString(colorNames[cmps.getColorID()], 0, 4);
LCD.drawString(r, 0, 5);
LCD.drawInt(cmps.getRGBComponent(Color.RED), 1, 5);
LCD.drawString(g, 5, 5);
LCD.drawInt(cmps.getRGBComponent(Color.GREEN), 6, 5);
LCD.drawString(b, 10, 5);
LCD.drawInt(cmps.getRGBComponent(Color.BLUE), 11, 5);
LCD.refresh();
Thread.sleep(INTERVAL);
}
}
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 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[] args) throws Exception {
RemoteNXT nxt = null;
int power = 0;
int mode = 1;
int motor = 0;
String motorString = "Motor:";
String modeString = "Mode:";
String powerString = "Power:";
String batteryString = "Battery:";
String lightString = "Light:";
String tachoString = "Tacho:";
// Get the type of communications to be used
String[] connectionStrings = new String[] {"Bluetooth", "RS485"};
TextMenu connectionMenu = new TextMenu(connectionStrings, 1, "Connection");
NXTCommConnector[] connectors = {Bluetooth.getConnector(), RS485.getConnector()};
int connectionType = connectionMenu.select();
// Now connect
try {
LCD.clear();
LCD.drawString("Connecting...", 0, 0);
nxt = new RemoteNXT("NXT", connectors[connectionType]);
LCD.clear();
LCD.drawString("Type: " + connectionStrings[connectionType], 0, 0);
LCD.drawString("Connected", 0, 1);
Thread.sleep(2000);
} catch (IOException ioe) {
LCD.clear();
LCD.drawString("Conn Failed", 0, 0);
Thread.sleep(2000);
System.exit(1);
}
LCD.clear();
RemoteMotor[] motors = {nxt.A, nxt.B, nxt.C};
LightSensor light = new LightSensor(nxt.S2);
while (true) {
// Get data from the remote NXT and display it
LCD.drawString(motorString, 0, 0);
LCD.drawInt(motor, 3, 10, 0);
LCD.drawString(powerString, 0, 1);
LCD.drawInt(power, 3, 10, 1);
LCD.drawString(modeString, 0, 2);
LCD.drawInt(mode, 3, 10, 2);
LCD.drawString(tachoString, 0, 3);
LCD.drawInt(motors[motor].getTachoCount(), 6, 7, 3);
LCD.drawString(batteryString, 0, 4);
LCD.drawInt(nxt.Battery.getVoltageMilliVolt(), 6, 7, 4);
LCD.drawString(lightString, 0, 5);
LCD.drawInt(light.readValue(), 6, 7, 5);
LCD.drawString(nxt.getBrickName(), 0, 6);
LCD.drawString(nxt.getFirmwareVersion(), 0, 7);
LCD.drawString(nxt.getProtocolVersion(), 4, 7);
LCD.drawInt(nxt.getFlashMemory(), 6, 8, 7);
// Do we have a button press?
int key = Button.readButtons();
if (key != 0) {
// New command, work out what to do.
if (key == 1) { // ENTER
power += 20;
if (power > 100) power = 0;
} else if (key == 2) { // LEFT
mode++;
if (mode > 4) mode = 1;
} else if (key == 4) { // RIGHT
motor++;
if (motor > 2) motor = 0;
} else if (key == 8) { // ESCAPE
LCD.clear();
LCD.drawString("Closing...", 0, 0);
for (int i = 0; i < motors.length; i++) motors[i].flt();
nxt.close();
Thread.sleep(2000);
System.exit(0);
}
LCD.clear();
LCD.drawString("Setting power", 0, 0);
motors[motor].setPower(power);
LCD.drawString("Moving motor", 0, 1);
if (mode == 1) motors[motor].forward();
else if (mode == 2) motors[motor].backward();
else if (mode == 3) motors[motor].flt();
else if (mode == 4) motors[motor].stop();
// Wait for the button to be released...
while (Button.readButtons() != 0) Thread.yield();
LCD.clear();
}
}
}
public void action() {
LCD.clear();
LCD.drawInt(sonar.getDistance(), 0, 0);
LCD.refresh();
}
