import lejos.nxt.*;
import lejos.robotics.navigation.*;

/**
 Robot that, three times: 
    goes forward a set distance 
    UNLESS it hits something on the way,
    in which case it backs up and stops

  @author www.lejos.org
  @author adapted by S. Tuttle
  @author impl'd by <present team member names>
  @version 2015-02-15 (completed after class)
**/

public class TravelTest
{
    // data fields

    private DifferentialPilot pilot;
    private TouchSensor bump = new TouchSensor(SensorPort.S3);

    /**
       each time this is called, go forward either 1000
       millimeters or until bumping into something
    **/

    public void go()
    {
        // try to travel 1000 mm (because called DifferentialPilot
        //    constructor in main with measurements given in mm)
        // (BUT return control to this method immediately)

        pilot.travel(1000, true);

        // if touch sensor registers as bumped before
        //     1000 mm traveled, go backward 200 mm
	//     and stop

        while (pilot.isMoving())
        {
            if (bump.isPressed())
            {
                System.out.println("PRESSED!: " +
                   pilot.getMovement().getDistanceTraveled());

                // go backward 200 mm and stop

                pilot.travel(-200);          
                pilot.stop();
            }
        }

        System.out.println(
            pilot.getMovement().getDistanceTraveled());
        Button.waitForAnyPress();
    }

    /** 
     set up an instance of THIS class
     and make it go 3 times

     @param args not used
    */

    public static void main(String[] args)
    {
        TravelTest traveler = new TravelTest();

        // happening to give wheel diameter and track distance
	//    (distance between center of the two wheel)
        //    in millimeters; the f below means I want float
	//    versions of those named constants, which is the type
	//    the constructor wants for those)
        // whatever units you use for these HERE will affect
	//    the units assumed in other DifferentialPilot
	//    methods such as travel (used in go method above)

        traveler.pilot = 
            new DifferentialPilot( 56f, 120f,
                   Motor.B, Motor.C);
        System.out.println("about to call go 1st time");
        Button.waitForAnyPress();
        traveler.go();

        System.out.println("about to call go 2nd time");
        Button.waitForAnyPress();
        traveler.go();

        System.out.println("about to call go 3rd time");
        Button.waitForAnyPress();
        traveler.go();
    }
}