123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161 |
- package robots;
- import java.util.ArrayList;
- import java.util.Random;
- import lejos.robotics.mapping.LineMap;
- import lejos.robotics.navigation.Pose;
- public class VirtualRobot implements Robot {
- private Pose pose;
- private Simulate simthread;
- private RobotReturn rr;
- private LineMap map;
- private int angle = 0;
- private Random rand;
- private class Simulate extends Thread {
- public boolean run = true;
- public void run() {
- angle = 0;
- int add = -5;
- while (run) {
- DataPose data = new DataPose();
- double dist = getDistance();
- data.setDistance((int) dist);
- data.setPose(pose);
- data.setSensorAngle(angle + 90);
- rr.robotData(data);
- angle += add;
- if (angle == -180 || angle == 0)
- add *= -1;
- try {
- Thread.sleep(50);
- } catch (InterruptedException e) {
- }
- }
- }
- }
- public double getDistance() {
- Pose tmppose = new Pose(pose.getX(), pose.getY(), pose.getHeading());
- float mindist = Float.POSITIVE_INFINITY;
- int cone = 30;
- for (int angulo = -cone / 2; angulo <= cone / 2; angulo++) {
- tmppose.setHeading((float) (pose.getHeading() - angulo + angle));
- float dist = map.range(tmppose);
- if (dist > 0 && dist < mindist)
- mindist = dist;
- }
- double v = mindist + rand.nextGaussian() * 4 + Math.random() * 2;
- v = Math.min(255, v);
- return v;
- }
- public VirtualRobot(LineMap map) {
- pose = new Pose();
- pose.setHeading(0);
- this.map = map;
- rand = new Random();
- }
- @Override
- public void moveForward() {
- move(5);
- }
- @Override
- public void moveLeft() {
- pose.rotateUpdate(45);
- }
- @Override
- public void moveRight() {
- pose.rotateUpdate(-45);
- }
- @Override
- public void moveBackward() {
- move(-5);
- }
- @Override
- public boolean connect() {
- return true;
- }
- @Override
- public void stop() {
- }
- @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);
- }
- @Override
- public void rotate(double x) {
- pose.rotateUpdate((float) -x);
- }
- @Override
- public ArrayList<DataPose> scann(int ini, int end, int interval) {
- ArrayList<DataPose> result = new ArrayList<DataPose>();
- for (angle = ini; angle <= end; angle += interval) {
- DataPose data = new DataPose();
- double dist = getDistance();
- data.setDistance((int) dist);
- data.setPose(pose);
- data.setSensorAngle(angle);
- result.add(data);
- }
- return result;
- }
- @Override
- public void scann(RobotReturn r) {
- rr = r;
- stopScann();
- simthread = new Simulate();
- simthread.start();
- }
- @Override
- public void stopScann() {
- if (simthread == null)
- return;
- simthread.run = false;
- try {
- simthread.join();
- } catch (InterruptedException e) {
- // TODO Auto-generated catch block
- e.printStackTrace();
- }
- }
- @Override
- public void disconnect() {
- }
- @Override
- public void setPose(float x, float y, float a) {
- pose.setHeading(a);
- pose.setLocation(x, y);
- }
- @Override
- public String toString() {
- return "Virtual Robot";
- }
- }
|