123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129 |
- package robots;
- import java.util.ArrayList;
- import lejos.robotics.navigation.Pose;
- import lejos.robotics.navigation.DifferentialPilot;
- import lejos.nxt.Motor;
- import lejos.nxt.SensorPort;
- import lejos.nxt.UltrasonicSensor;
- import lejos.nxt.remote.RemoteMotor;
- public class LCPRobot implements Robot {
- private Pose pose;
- static DifferentialPilot pilot;
- static UltrasonicSensor sonar;
- RemoteMotor scannerm = Motor.A;
- RemoteMotor ma = Motor.B;
- RemoteMotor mb = Motor.C;
- public LCPRobot() {
- }
- @Override
- public void move(double 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);
- pilot.travel(x);
- }
- @Override
- public void rotate(double x) {
- pose.rotateUpdate((float)-x);
- 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;
- int engine_mult = 1;
- scannerm.setSpeed(200);
- scannerm.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
- scannerm.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
- scannerm.setSpeed(80);
- for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
- scannerm.rotate(-(engine_mult * interval));
- val = sonar.getDistance();
- // ang = (j/(5*end)) * 180.0;
- ang = ini + interval;
- DataPose data = new DataPose();
- data.setDistance(val);
- data.setSensorAngle(ang);
- data.setPose(pose);
- result.add(data);
- }
- scannerm.setSpeed(200);
- scannerm.rotate(0);
- 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() {
- //pilot = new DifferentialPilot(5.6f, 11.2f, Motor.B, Motor.A);
- pilot = new DifferentialPilot(5.6f, 11.2f, ma, mb);
- sonar = new UltrasonicSensor(SensorPort.S1);
- pilot.setTravelSpeed(10);
- pilot.setRotateSpeed(40);
- return true;
- }
- @Override
- public void disconnect() {
- }
- @Override
- public void setPose(float x, float y, float a) {
- pose.setHeading(a);
- pose.setLocation(x, y);
- }
- }
|