123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120 |
- package robots;
- import java.util.ArrayList;
- import lejos.robotics.localization.OdometryPoseProvider;
- import lejos.robotics.navigation.Pose;
- import lejos.robotics.navigation.DifferentialPilot;
- import lejos.nxt.Motor;
- import lejos.nxt.SensorPort;
- import lejos.nxt.UltrasonicSensor;
- public class LCPRobot implements Robot {
- private Pose pose;
- static DifferentialPilot pilot;
- static UltrasonicSensor sonar;
- public LCPRobot() {
- }
- @Override
- public void move(double x) {
- pilot.travel(x);
- }
- @Override
- public void rotate(double 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;
- Motor.C.setSpeed(200);
- Motor.C.rotate(-90 * engine_mult); // gira sonar para a posicao 0 grau
- Motor.C.rotate(-(engine_mult * ini)); // gira ate a posicao inicial passada no parametro ini
- Motor.C.setSpeed(80);
- for (int j = 0; j <= (engine_mult * end); j += (engine_mult * interval)) {
- Motor.C.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(provider.getPose());
- result.add(data);
- }
- Motor.C.setSpeed(200);
- Motor.C.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, Motor.B, Motor.A);
- 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) {
- provider.setPose(new Pose(x, y, a));
- }
- }
|