MAC0318/.metadata/.plugins/org.eclipse.core.resources/.history/f7/a0e545ba20bb001711b8bf632416c20d

package robots;

import java.util.ArrayList;

import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.mapping.LineMap;
import lejos.robotics.navigation.Pose;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.nxt.Motor;
import lejos.util.Delay;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;

public class LCPRobot implements Robot {
	OdometryPoseProvider provider;

    static DifferentialPilot pilot;
    static UltrasonicSensor sonar;

    public LCPRobot () {
        pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
        sonar = new UltrasonicSensor(SensorPort.S1);
        pilot.setTravelSpeed(10);
        pilot.setRotateSpeed(40);
        provider = new OdometryPoseProvider(pilot);
    }

    @Override
    public void move(double x) {
    		pilot.travel(x);
    }

    @Override
    public void rotate(double x) { // espera um valor positivo ou negativo entre 0 e 360
        pilot.rotate(x);
    }

    @Override
    public ArrayList<DataPose> scann(int ini, int end, int interval) {
        ArrayList<DataPose> result = new ArrayList<DataPose>();
        int val = 0;
        double ang = 0;
        Motor.C.setSpeed(200);
        Motor.C.rotate(450); // gira sonar para a posicao 0 grau
        Motor.C.rotate(-(5*ini)); // gira ate a posicao inicial passada no parametro ini
        Motor.C.setSpeed(80);
        for (int j=0; j <= (5*end); j+=(5*interval)){ 
            DataPose data = new DataPose();
            Motor.C.rotate(-(5*interval));
            val = sonar.getDistance();
            //ang = (j/(5*end)) * 180.0;
            ang = ini + interval;
            data.setDistance(val);
            data.setSensorAngle(ang);
            data.setPose(provider.getPose());
            result.add(data);
        }
        Motor.C.setSpeed(200);
        Motor.C.rotate(460);
        return result;
    }
   
    @Override 
    public String toString() {
        return "LCP Robot";
    }

	@Override
	public void moveForward() {
		pilot.forward();
		
	}

	@Override
	public void moveLeft() {
		pilot.rotateLeft();
	}

	@Override
	public void moveRight() {
		pilot.rotateRight();
	}

	@Override
	public void moveBackward() {
		pilot.backward();
		
	}

	@Override
	public void stop() {
		pilot.stop();
		
	}

	@Override
	public void scann(RobotReturn r) {
		// TODO Auto-generated method stub
		
	}

	@Override
	public void stopScann() {
		// TODO Auto-generated method stub
		
	}

	@Override
	public boolean connect() {
		return true;
	}

	@Override
	public void disconnect() {		
	}

	@Override
	public void setPose(float x, float y, float a) {
		provider.setPose(new Pose(x, y, a));
	}

}