123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139 |
- package robots;
- import java.util.ArrayList;
- 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 class Simulate extends Thread {
- public boolean run = true;
- public void run() {
- int ang = 0;
- int add = -5;
- while (run) {
- DataPose data = new DataPose();
-
- Pose tmppose = new Pose(pose.getX(), pose.getY(), (float) (pose.getHeading() + ang));
- float dist = map.range(tmppose);
- if (dist == -1) dist = 255;
-
-
- data.setDistance((int)dist);
- data.setPose(pose);
- data.setSensorAngle(ang);
- rr.robotData(data);
- ang += add;
- if (ang == -180 || ang == 0)
- add *= -1;
- try {
- Thread.sleep(50);
- } catch (InterruptedException e) {
- }
- }
- }
- }
- public VirtualRobot(LineMap map) {
- pose = new Pose();
- pose.setHeading(90);
- this.map = map;
- }
- @Override
- public void moveForward() {
- double dx = Math.sin(Math.toRadians(pose.getHeading())) * 5;
- double dy = Math.cos(Math.toRadians(pose.getHeading())) * 5;
- pose.translate((float) dx, (float) -dy);
- }
- @Override
- public void moveLeft() {
- pose.rotateUpdate(45);
- }
- @Override
- public void moveRight() {
- pose.rotateUpdate(-45);
- }
- @Override
- public void moveBackward() {
- double dx = Math.sin(Math.toRadians(pose.getHeading())) * 5;
- double dy = Math.cos(Math.toRadians(pose.getHeading())) * 5;
- pose.translate((float) -dx, (float) dy);
- }
- @Override
- public boolean connect() {
- return true;
- }
- @Override
- public void stop() {
- }
- @Override
- public void move(double x) {
- // TODO Auto-generated method stub
- }
- @Override
- public void rotate(double x) {
- // TODO Auto-generated method stub
- }
- @Override
- public ArrayList<DataPose> scann(int ini, int end, int interval) {
- // TODO Auto-generated method stub
- return null;
- }
- @Override
- public void scann(RobotReturn r) {
- rr = r;
- simthread = new Simulate();
- simthread.start();
- }
- @Override
- public void stopScann() {
- 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";
- }
- }
|