import lejos.navigation.Pilot;
import lejos.navigation.TachoNavigator;
import lejos.nxt.Motor;

/**
 * Code for Nano table.
 *
 * We chose not to implement behaviours at the Nano table.
 * 
 * @author Hadaga
 */
public class Nano
{
    private static final int WHEEL_DIAMETER = 56;
    private static final int TRACK_WIDTH = 135;
    
    public static void main (String[] args) throws InterruptedException
    {
        /** http://lejos.sourceforge.net/tutorial/navigation/index.html
        We need to balance the motors--if they are not balanced, the robot
        will be thrown off course. */
        Motor.A.setPower(100);
        Motor.B.setPower(100);

        // Cumulative speed error is within about 1 degree after initial
        // acceleration
        Motor.A.regulateSpeed(true);
        Motor.B.regulateSpeed(true);
        Motor.C.regulateSpeed(true);
        
        // Smooth acceleration to increase motor speed gently
        Motor.A.smoothAcceleration(true);
        Motor.B.smoothAcceleration(true);
        Motor.C.smoothAcceleration(true);
        
        // Creates pilot, boolean set to true because motors are reversed
        Pilot pilot = new Pilot(WHEEL_DIAMETER, TRACK_WIDTH, Motor.A, Motor.B);
        
        // Creates TachoNavigator to track robot position and direction angle
        // it faces
        TachoNavigator robot = new TachoNavigator(pilot);
        
        // Robot speed in revolutions per second (900 max with 8 volts)
        robot.setSpeed(720);
        
        // Robot is at origin, facing +Y axis (recall 0 is +X, 180 is -X...)
        robot.setPosition(95, 110, 90);
        
        Thread.sleep(2000);
        
        // Go to ball 1
        robot.goTo(530, 800);
        
        // Head right till balls are cleared
        robot.goTo(1280, 750);
        
        // Reverse to clear Fabric
        robot.travel(-160);
        robot.turn(-100, 90);
        
        // Arc to bump far elevator
        robot.turn(230, 180);
        robot.travel(40);
        
        // Backup
        robot.travel(-60);
        
        // Bump ball
        robot.goTo(1800, 700);
        
        // Reverse to center point
        robot.travel(-550);
        
        // Rotate so tail attachment is in line with Self Assembly
        robot.rotateTo(160);
        
        // Reverse to mechanism
        Motor.C.rotate(-115);
        robot.travel(-410);
        
        // Sweep to activate mechanism
        robot.rotateTo(100);
        robot.rotateTo(160);
        
        // Move to checkpoint (circle on arm)
        robot.travel(700);
        Motor.C.rotate(115);
        
        // Return to base
        robot.goTo(0, 0);   
    }
}