/***
 * EdgeFollower is a LeJOS robot that follows the edge of a black/white line.
 *
 * To use it you'll need an RCX lego robot with 
 * two motors (on ports A,B, one for each wheel) 
 * and one light sensor (on port S3, looking down).
 *
 * Compile and download the code as usual.
 *      $ lejosc EdgeFollower.java
 * Then turn on lego brick and aim it at the IR tower.
 *      $ lejos EdgeFollower
 *
 * Put the robot over the white color, and push "Run".
 * When it beeps, move quickly it to the black color near the edge.
 *
 * It should then follow the border, waggling back and forth
 * runnning either the left or right wheel depending on whether
 * the sensor sees white or black.
 *
 * @version 1.1 Jan 2007
 * @author Jim Mahoney
 *
 ****/
import josx.platform.rcx.*;

class EdgeFollower {
    public static int whiteValue;
    public static int blackValue;

    public static void main(String[] args) throws Exception {
        initialize();
        whiteValue = getSensor("Wh", 1);  // message, wait time (sec)
        blackValue = getSensor("Bl", 2);
        TextLCD.print("--");
        while (true){
            int howFarFromWhite = getSensorDifference(whiteValue);
            int howFarFromBlack = getSensorDifference(blackValue);
            if (howFarFromWhite < howFarFromBlack){
                Motor.A.backward();
                Motor.B.stop();
            }
            else {
                Motor.A.stop();
                Motor.B.forward();
            }        
            Thread.sleep(50);//  slow the loop to avoid excess robot wiggling
        } // end while
    } // end main
    
    /**
     * Return the difference between the sensor and reference
     * @param referenceValue     typically the white or black value
     */
    public static int getSensorDifference(int referenceValue){
        return Math.abs(Sensor.S3.readValue() - referenceValue);
    }
    
    /**
     * Display a text message, wait, beep, and return the value of the sensor.
     * @param message    a short string to display in the lego brick's LCD
     * @param sleepSec   how long to wait (in seconds)
     */
    public static int getSensor(String message, int sleepSec) throws Exception{
        TextLCD.print(message);
        Thread.sleep(1000*sleepSec);
        Sound.beep();
        return Sensor.S3.readValue();
    }
    
    /**
     * Initialize the lego brick's sensors and motors.
     */
    public static void initialize(){
        Sensor.S3.setTypeAndMode(SensorConstants.SENSOR_TYPE_LIGHT, 
                                 SensorConstants.SENSOR_MODE_PCT);
        Sensor.S3.activate();
        Motor.A.setPower(7);
        Motor.B.setPower(7);
    }

}