lego

package lalab;

import java.io.IOException;
import java.util.ArrayList;

import lejos.geom.Line;
import lejos.geom.Rectangle;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.mapping.LineMap;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Pose;
import lejos.robotics.navigation.Waypoint;
import lejos.util.Delay;
import lejos.util.PilotProps;
import lejos.robotics.pathfinding.NodePathFinder;
import lejos.robotics.pathfinding.Path;
import lejos.robotics.pathfinding.ShortestPathFinder;
import lejos.robotics.pathfinding.NavigationMesh;
import lejos.robotics.pathfinding.FourWayGridMesh;
import lejos.robotics.pathfinding.AstarSearchAlgorithm;
import lejos.robotics.pathfinding.DijkstraPathFinder;

public class Move
{

	public static 	Line[] lines = {

			new Line(-40,-42,-40,102),
			new Line(-42,100,22,100),
			new Line(20,102,20,78),
			new Line(18,80,82,80),
			new Line(80,78,80,122),
			new Line(78,120,122,120),
			new Line(120,122,120,78),
			new Line(118,80,162,80),
			new Line(160,82,160,-42),
			new Line(162,-40,-42,-40),
		};


	static RegulatedMotor leftMotor;
	static RegulatedMotor rightMotor;

	public static Pose start = new Pose(0,0,180);
	public static Pose final_ = new Pose(100,100,90);


	/*public static Waypoint end_node_waypoint = new Waypoint(final_);
	public static Rectangle ramka = new Rectangle(150,150,250,250);
	public static LineMap mapa = new LineMap(lines,ramka);
	public static DijkstraPathFinder dijkstra = new DijkstraPathFinder(mapa);
	public static Path path;
	public static String pathString;
	*/

	public static void main(String[] args ) throws Exception
	{

    	PilotProps pp = new PilotProps();
    	pp.loadPersistentValues();
    	float wheelDiameter = Float.parseFloat(pp.getProperty(PilotProps.KEY_WHEELDIAMETER, "4.96"));
    	float trackWidth = Float.parseFloat(pp.getProperty(PilotProps.KEY_TRACKWIDTH, "13.0"));
    	leftMotor = PilotProps.getMotor(pp.getProperty(PilotProps.KEY_LEFTMOTOR, "A"));
    	rightMotor = PilotProps.getMotor(pp.getProperty(PilotProps.KEY_RIGHTMOTOR, "B"));
    	boolean reverse = Boolean.parseBoolean(pp.getProperty(PilotProps.KEY_REVERSE,"false"));
    	DifferentialPilot robot = new DifferentialPilot(wheelDiameter,trackWidth,leftMotor,rightMotor,reverse);

        // Wait for user to press ENTER
		Button.waitForAnyPress();


		robot.setAcceleration(4000);
		robot.setTravelSpeed(20); // cm/sec
		robot.setRotateSpeed(180); // deg/sec
		robot.travel(100,true);
		while(robot.isMoving())
			Thread.yield();
		robot.rotate(90);

		robot.travel(100,true);
		while(robot.isMoving())
			Thread.yield();

		robot.rotate(90);
		robot.travel(100,true);
		while(robot.isMoving())
			Thread.yield();
		robot.rotate(90);
		robot.travel(100,true);


		// robot.forward();

		/*
		 * Delay.msDelay(1000);;

		robot.stop();
		showCount(robot, 0);

		robot.backward();
		Delay.msDelay(1000);;

		robot.stop();
		showCount(robot, 1);
		robot.travel(10,true);

		while(robot.isMoving())
		Thread.yield();
		showCount(robot, 2);
		robot.travel(-10);
		showCount(robot, 3);

		for(int i = 0; i<4; i++)
		{
			robot.rotate(90);
		}

		showCount(robot, 4);

		for(int i = 0; i<4; i++)
		{
			robot.rotate(-90,true);
			while(robot.isMoving())
				Thread.yield();
		}
		showCount(robot, 5);
		robot.steer(-50,180,true);

		while(robot.isMoving())
			Thread.yield();

		robot.steer(-50,-180);
		showCount(robot, 6);
		robot.steer(50,180);
		robot.steer(50, -180);
		showCount(robot, 7);
		robot.travel(10,true);
		Delay.msDelay(500);
                robot.stop();
		robot.travel(-10);
		robot.rotate(720);
		*/
		// Exit after any button is pressed
		Button.waitForAnyPress();
	}

	public static void showCount(DifferentialPilot robot, int i)
	{
		LCD.drawInt(leftMotor.getTachoCount(),0,i);
		LCD.drawInt(rightMotor.getTachoCount(),7,i);
	}

	public void stop() {
		//while(Button.waitForAnyPress()) {


	}

}