MAC0318/.metadata/.plugins/org.eclipse.core.resources/.history/68/d062701a20bb001711b8bf632416c20d

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);
    }

    static double theta = 0;
    static double x = 0;
    static double y = 0;

    @Override
    public void move(double x) {
        pilot.travel(x);
        x += Math.cos(Math.toRadians(theta)) * x;
        y += Math.sin(Math.toRadians(theta)) * x;
        // double dx = Math.sin(Math.toRadians(pose.getHeading())) * x;
        // double dy = Math.cos(Math.toRadians(pose.getHeading())) * x;
        // pose.translate((float) dx, (float) -dy);
    }

    @Override
    public void rotate(double x) { // espera um valor positivo ou negativo entre 0 e 360
        pilot.rotate(x);
        if ((theta + x) > 360){
            int add = (theta + x) % 360;
            theta += add;
        }
        else if ((theta + x) < 0){
            theta = 360 + (theta + x);
        }
        else
            theta += x;
    }

    @Override
    public ArrayList<DataPose> scann(int ini, int end, int interval) {
        ArrayList<DataPose> result = new ArrayList<DataPose>();
        Pose p = new Pose((float) x, (float) y, (float) theta);
        int val = 0;
        int i=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(p);
            result.add(data);
        }
        Motor.C.setSpeed(200);
        Motor.C.rotate(460);
        return result;
    }
   
    @Override 
    public String toString() {
        return "LCP Robot";
    }

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

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

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

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

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

	@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) {
		// TODO Auto-generated method stub
		
	}

}