123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148 |
- package robots;
- import java.util.ArrayList;
- 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 {
- //private Pose pose;
- //private Simulate simthread;
- private RobotReturn rr;
- private LineMap map;
- 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);
- }
- 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() {
- // TODO Auto-generated method stub
-
- }
- @Override
- public void setPose(float x, float y, float a) {
- // TODO Auto-generated method stub
-
- }
- }
|