import lejos.nxt.*;/** * A simple line follower for the LEGO 9797 car with * a light sensor. Before the car is started on a line * a BlackWhiteSensor is calibrated to adapt to different * light conditions and colors. * * The light sensor should be connected to port 3. The * left motor should be connected to port C and the right * motor to port B. * * @author Ole Caprani * @version 23.08.07 */public class LineFollowerCal{ static ColorSensor rightSensor; static ColorSensor leftSensor; static ColorSensor rightMiddleSensor; static ColorSensorNew leftMiddleSensor; public static void read(String color){ int rightLightValue = 0; int leftLightValue = 0; int leftMiddleLightValue = 0; int rightMiddleLightValue = 0; while(Button.ENTER.isPressed() ){} LCD.clear(); LCD.drawString("Press ENTER", 0, 0); LCD.drawString("to callibrate", 0, 1); LCD.drawString(color, 0, 2); while( !Button.ENTER.isPressed() ){ rightLightValue = rightSensor.light(); leftLightValue = leftSensor.light(); leftMiddleLightValue = leftMiddleSensor.light(); rightMiddleLightValue = rightMiddleSensor.light(); LCD.drawInt(leftLightValue, 4, 10, 1); LCD.drawInt(leftMiddleLightValue, 4, 10, 2); LCD.drawInt(rightMiddleLightValue, 4, 10, 3); LCD.drawInt(rightLightValue, 4, 10, 4); LCD.refresh(); } rightSensor.setColor(color,rightLightValue); leftSensor.setColor(color,leftLightValue); rightMiddleSensor.setColor(color,rightMiddleLightValue); leftMiddleSensor.setColor(color,leftMiddleLightValue); } public static void calibrate() { read("black"); read("white"); read("blue"); } public static void main (String[] aArg) throws Exception { final int power = 100; final int leftBlackValue = 23; final int leftWhiteValue = 34; final int rightBlackValue = 24; final int rightWhiteValue = 35; final int RIGHT = 0; final int FORWARD = 1; final int LEFT = 2; final int blueValue = 29; //StopWatch sw = new StopWatch(); rightSensor = new ColorSensor(SensorPort.S2,rightBlackValue,rightWhiteValue,blueValue); leftSensor = new ColorSensor(SensorPort.S3,leftBlackValue,leftWhiteValue,blueValue); leftMiddleSensor = new ColorSensorNew(SensorPort.S4,leftBlackValue,leftWhiteValue,blueValue); rightMiddleSensor = new ColorSensor(SensorPort.S1,leftBlackValue,leftWhiteValue,blueValue); // leftSensor.calibrate(); // rightSensor.calibrate(); calibrate(); // DataLogger dlRight = new DataLogger("BWRight.txt"); // DataLogger dlLeft = new DataLogger("BWLeft.txt"); LCD.clear(); LCD.drawString("Press ENTER to start", 0, 0); while (Button.ENTER.isPressed()){} while (! Button.ENTER.isPressed()){} //sw.start(); int lastDirection = FORWARD; while (! Button.ESCAPE.isPressed()) { int rightLight = rightSensor.light(); int leftLight = leftSensor.light(); LCD.drawInt(rightLight,4,10,2); LCD.drawInt(leftLight,4,10,3); LCD.refresh(); boolean leftBlack = leftSensor.black(); boolean rightBlack = rightSensor.black(); boolean leftMiddleBlack = leftMiddleSensor.black(); boolean rightMiddleBlack = rightMiddleSensor.black(); boolean leftWhite = leftSensor.white(); boolean rightWhite = rightSensor.white(); boolean leftMiddleWhite = leftMiddleSensor.white(); boolean rightMiddleWhite = rightMiddleSensor.white(); if (leftMiddleBlack && rightMiddleBlack) { Car.forward(power, power); } else if(leftMiddleBlack && rightMiddleWhite) { Car.forward(85, power); } else if(leftMiddleWhite && rightMiddleBlack) { Car.forward(power,85); } else if(leftBlack) { //hard left Car.forward(0, power); Car.backwardLeft(40); } else if(rightBlack) { //hard right Car.forward(power,0); Car.backwardRight(40); } if( leftSensor.blue() || (leftMiddleSensor.blue() && rightMiddleSensor.blue()) || rightSensor.blue()) { break; //sw.stop(); } //dlRight.writeSample(rightLight); //dlLeft.writeSample(leftLight); Thread.sleep(1); } Car.stop(); LCD.clear(); int timePassed = 0;//sw.getTimeElapsed(); LCD.drawInt(timePassed,4,10,2); while (! Button.ENTER.isPressed()){} LCD.drawString("Program stopped", 0, 0); LCD.refresh(); }}