Race2.java

import lejos.nxt.*;
 
public class Race2 {
 
    private static int power = 75;
    private static int minPower = 50;
 
    private static ColorSensor lineSensor;
    private static ColorSensor otherSensor;
    private static int leftFastPower;
    private static int rightFastPower;
    private static int leftSlowPower;
    private static int rightSlowPower;
    private static int stateCounter = 0;
 
    private static final int f = 256, d = 500, v = 100; 
 
    private static ColorSensor leftSensor = new ColorSensor(SensorPort.S1);
    private static ColorSensor rightSensor = new ColorSensor(SensorPort.S4);
 
    public static void main (String[] aArg) throws Exception {
 
        MotorPort.A.controlMotor(100, 1);
        Thread.sleep(1000);
        //        turnAround();
        leftSensor.calibrate();
        rightSensor.calibrate();
        boolean compensateForStateFiveError = true;
        boolean compensateForStateFourError = true;
 
        LCD.clear();
        LCD.drawString("Light: ", 0, 2); 
 
        Thread.sleep(500);
 
        while (! Button.ENTER.isPressed());
 
        // get out of the green zone.
        Car.forward(60, 60);
        Thread.sleep(800);
        switchState();
        int internalCounter = 0;
        while (! Button.ESCAPE.isPressed() && stateCounter < 9) {
            LCD.drawInt(lineSensor.readValue(), 4, 10, 2);
            LCD.refresh();
 
            internalCounter++;
            if (internalCounter > 1000 && otherSensor.black()) {
                /*if (stateCounter == 5 && compensateForStateFiveError) {
                    compensateForStateFiveError = false;
                    internalCounter = 980;
                    Sound.playTone(6*f, d, v);
                } else if (stateCounter == 4 && compensateForStateFourError) {
                    compensateForStateFourError = false;
                    internalCounter = 980;
                    Sound.playTone(6*f, d, v);
                } else {*/ 
                switchState();
                internalCounter = 0; 
                //                }
            }
            if (lineSensor.gray()) {
                LCD.drawString("gray ", 0, 4);
                if (leftSensor.getError() < 0) {
                    Car.forward(leftFastPower, rightFastPower);
                } else {
                    Car.forward(rightFastPower, leftFastPower);
                }
            } else if ( lineSensor.white() ) {
                LCD.drawString("white", 0, 4);
                Car.forward(leftSlowPower, rightSlowPower);
            } else if ( lineSensor.black() ) {
                LCD.drawString("black", 0, 4);
                Car.forward(rightSlowPower, leftSlowPower);
            }
            Thread.sleep(5);
        }
 
        Car.stop();
        LCD.clear();
        LCD.drawString("Program stopped", 0, 0);
        LCD.refresh();
    }
 
    private static void switchState() throws InterruptedException {
        Sound.playTone(2*f, d, v);
        switch (stateCounter) {
        case 3: 
            // turn around and reduce the powers.
            turnAround();
            break;
        case 2:
        case 5:
            Car.forward(0, 75);
            Thread.sleep(1550);
        case 0:
            // follow the line with the left sensor
            lineSensor = leftSensor; 
            otherSensor = rightSensor;
            rightFastPower = minPower;
            leftFastPower = power;
            rightSlowPower = 0;
            leftSlowPower = power;
            break;
        /*case 4:
            // follow the line with the right sensor
            lineSensor = rightSensor; 
            otherSensor = leftSensor;
            rightFastPower = power;
            leftFastPower = minPower;
            rightSlowPower = power;
            leftSlowPower = 0;
            Car.forward(70, 70);
            Thread.sleep(200);
            Car.forward(70, 0);
            Thread.sleep(1800);
            break;*/
        /*case 5:
            // follow the line with the left sensor
            lineSensor = leftSensor; 
            otherSensor = rightSensor;
            rightFastPower = minPower;
            leftFastPower = power;
            rightSlowPower = 0;
            leftSlowPower = power;
            Car.forward(70, 70);
            Thread.sleep(200);
            Car.forward(0, 70);
            Thread.sleep(1700);
            break;*/
        case 4:
        case 1:
            // follow the line with the right sensor
            lineSensor = rightSensor; 
            otherSensor = leftSensor;
            rightFastPower = power;
            leftFastPower = minPower;
            rightSlowPower = power;
            leftSlowPower = 0;
            Car.forward(75, 0);
            Thread.sleep(1450);
            break;
        default: 
            Car.forward(70, 70);
            Thread.sleep(1500);
        }    
        Sound.playTone(2*f, d, v);
        stateCounter++;
        LCD.drawString("state: " + stateCounter, 0, 5);
    }
 
    private static void turnAround() throws InterruptedException {
        Sound.playTone(3*f, d, v);
        // get into the turn area
        Car.forward(70, 70);
        Thread.sleep(1300);
        //        Car.forward(100, 100);
        //        Thread.sleep(500);
        // turn around
        Car.forward(-80, 80);
        Thread.sleep(700);
        // get out
        Car.forward(70, 70);
        Thread.sleep(1300);
        Sound.playTone(3*f, d, v);
 
        // reduce the powers
        power = 71;
        minPower = 40;
        rightFastPower = minPower;
        leftFastPower = power;
        rightSlowPower = 0;
        leftSlowPower = power;
    }
}