123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124 |
- 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;
- double 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(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));
- }
- }
|