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 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();
}
@Override
protected void executeWithConnected(SoccerRobot robot) {
while (!Button.ESCAPE.isPressed()) {
if (robot.hasBall()) robot.kick();
try {
Thread.sleep(250);
} catch (InterruptedException e) {
}
}
}
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 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();
}
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 {
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);
}
}
@Override
public void run() {
// TODO Auto-generated method stub
while (!Button.ENTER.isDown()) ;
LCD.clear();
if (menuSelection == 1) {
LCD.drawString("Task 1-2 Selected", 0, 0);
} else if (menuSelection == 2) {
LCD.drawString("Task 3 Selected", 0, 0);
}
ultrasonicSensor.wakeUp();
display.wakeUp();
LCD.drawString("Threads Awake", 0, 1);
leftTunedSpeed = leftSpeed;
rightTunedSpeed = rightSpeed;
// switchGains();
LCD.drawString("Running...", 0, 2);
try {
Thread.sleep(50);
while (!Button.ESCAPE.isDown()) {
Thread.sleep(timeStep);
// if(menuSelection == 1)
// {
// if(Math.abs(lineError) > 200)
// {
// counter++;
// if(counter > 10)
// {
// switchGains();
// counter = 0;
// }
// }
// if(state == 0)
// {
// Sound.beep();
// }
// }
// else
// {
// counter++;
// if(counter > 50)
// {
// state = 0;
// switchGains();
// }
// }
// state = 0;
// switchGains();
// state = 0;
// else
// {
// counter = 0;
// }
rangeReading = ultrasonicSensor.getPulseLenght() / 1000;
// LCD.drawString("" + echoValue, 0, 4);
rangePower =
(rangeFinderPID.pid(echoTarget, rangeReading, (double) timeStep / 1000))
/ 40; // blah/40
position = IRArray.calculatePosition();
lineError =
10 * linePID.pid(0, position, (double) timeStep / 1000) / (3 * IR_MAX_ERROR); // 360
readings = IRArray.poleSensor();
if (menuSelection == 1) // Task 1-2: Line and Block Stop
{
if (rangeReading < 600) {
leftTunedSpeed = (int) (setPower - lineError - rangePower);
rightTunedSpeed = (int) (setPower + lineError - rangePower);
} else {
leftTunedSpeed = (int) (setPower - lineError);
rightTunedSpeed = (int) (setPower + lineError);
}
// if(echoValue < 300)
// {
// leftTunedSpeed /= 5;
// rightTunedSpeed /= 5;
// }
if (rangeReading < 400) {
stop = true;
} else {
stop = false;
}
} else if (menuSelection == 2) // Task 3: Platooning and Redline Stop
{
rangeFinderPID.capI(100);
// if(echoValue < echoTarget)
// {
// leftTunedSpeed = (int)(set - curveError - echoError)/5;
// rightTunedSpeed = (int)(set + curveError - echoError)/5;
// }
// else
// {
leftTunedSpeed = (int) (setPower - lineError - rangePower);
rightTunedSpeed = (int) (setPower + lineError - rangePower);
// }
if (readings[0] < 800
&& readings[0] > 400
&& readings[2] < 800
&& readings[2] > 400
&& readings[1] < 800
&& readings[1] > 400) {
stop = true;
}
}
if (stop) {
motors.stopMotors();
LCD.drawString("Stopped", 0, 3);
} else {
if (menuSelection == 1) motors.updateMotors(leftTunedSpeed, rightTunedSpeed);
else {
if (leftTunedSpeed < 0 && rightTunedSpeed < 0) {
motors.leftMotor.motor.backward();
motors.rightMotor.motor.backward();
leftTunedSpeed = setPower - leftTunedSpeed;
rightTunedSpeed = setPower - rightTunedSpeed;
} else {
motors.leftMotor.motor.forward();
motors.rightMotor.motor.forward();
}
motors.updateMotors(leftTunedSpeed, rightTunedSpeed);
}
}
}
} catch (InterruptedException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
public RaceMain() {
Button.ESCAPE.addButtonListener(this);
}