123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273 |
- package robots;
- import java.io.DataInputStream;
- import java.io.DataOutputStream;
- import java.io.IOException;
- import java.util.ArrayList;
- import java.util.concurrent.Semaphore;
- import lejos.pc.comm.NXTComm;
- import lejos.pc.comm.NXTCommException;
- import lejos.pc.comm.NXTCommFactory;
- import lejos.pc.comm.NXTInfo;
- import lejos.robotics.navigation.Pose;
- public class BluetoothRobot implements Robot {
- private String name;
- private NXTComm nxtComm;
- private RobotReturn rr;
- private Semaphore semaphore;
- private Semaphore countsema;
- private int creads = -1;
-
- public static final byte FORWARD = 0;
- public static final byte STOP = 1;
- public static final byte EXIT = 2;
- public static final byte LEFT = 3;
- public static final byte RIGHT = 4;
- public static final byte BACKWARD = 5;
-
- public static final byte STOPSCANN = 6;
- public static final byte STARTSCANN = 7;
- public static final byte MOVE = 8;
- public static final byte ROTATE = 9;
- public static final byte SETPOSE = 10;
- public static final byte SETSCANANGLE = 11;
-
- private static final int scannangle = 5;
- private DataOutputStream output;
- private DataInputStream input;
- private Receiver receivethread;
-
- private ArrayList<DataPose> reads;
-
- private class Receiver extends Thread {
- public boolean run = true;
- @Override
- public void run() {
- int bytes_valiable = -1;
-
- while(run) {
- try {
- bytes_valiable = input.available();
- } catch (IOException e1) {
- // TODO Auto-generated catch block
- e1.printStackTrace();
- }
- if (bytes_valiable >= 0) {
- try {
- if (input.readByte() != '@') continue;
- int angle = input.readByte();
- float alpha = input.readFloat();
- float x = input.readFloat();
- int distance = input.readByte();
- float y = input.readFloat();
- DataPose d = new DataPose();
- d.setDistance(distance);
- d.setSensorAngle(angle);
- d.setPose(new Pose(x, y, alpha));
- if (rr != null)
- rr.robotData(d);
- if (reads.size() <= creads) {
-
- } else {
- countsema.release();
- }
- } catch (IOException e1) {
- continue;
- }
- }
- }
- }
- }
- private void send(int command) {
- if (semaphore.tryAcquire()) {
- try {
- output.write(command);
- output.flush();
- } catch (IOException e) {
- System.out.println("Erro ao enviar...");
- }
- semaphore.release();
- }
- }
- private void send(int command, float f) {
- if (semaphore.tryAcquire()) {
- try {
- output.write(command);
- output.writeFloat(f);
- output.flush();
- } catch (IOException e) {
- System.out.println("Erro ao enviar...");
- }
- semaphore.release();
- }
- }
-
- private void send(int command, int i) {
- if (semaphore.tryAcquire()) {
- try {
- output.write(command);
- output.write(i);
- output.flush();
- } catch (IOException e) {
- System.out.println("Erro ao enviar...");
- }
- semaphore.release();
- }
- }
-
- private void send(Pose p) {
- if (semaphore.tryAcquire()) {
- try {
- output.write(SETPOSE);
- output.writeFloat(p.getX());
- output.writeFloat(p.getY());
- output.writeFloat(p.getHeading());
- output.flush();
- } catch (IOException e) {
- System.out.println("Erro ao enviar...");
- }
- semaphore.release();
- }
- }
-
- public BluetoothRobot (String name) {
- reads = new ArrayList<DataPose>();
- semaphore = new Semaphore(1);
- countsema = new Semaphore(1);
- receivethread = new Receiver();
- this.name = name;
- }
- @Override
- public void moveForward() {
- send(FORWARD);
- }
- @Override
- public void moveLeft() {
- send(LEFT);
- }
- @Override
- public void moveRight() {
- send(RIGHT);
- }
- @Override
- public void moveBackward() {
- send(BACKWARD);
- }
- @Override
- public boolean connect () {
- try {
- nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
- NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
- if (nxtInfo.length == 0) { // failed to find a brick with the ID
- System.err.println("NO NXT found");
- return false;
- }
- if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
- System.err.println("Failed to open NXT");
- return false;
- }
-
- input = new DataInputStream(nxtComm.getInputStream()); // open data input stream
- output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
- send(SETSCANANGLE, scannangle); // vai scanear em 5 em 5 graus
- } catch (NXTCommException e) {
- return false;
- }
-
- receivethread.start();
-
- return true;
- }
- @Override
- public void stop() {
- send(STOP);
- }
- @Override
- public void move(double x) {
- send(MOVE, (float)x);
- }
- @Override
- public void rotate(double x) {
- send(ROTATE, (float)x);
- }
- @Override
- public ArrayList<DataPose> scann(int ini, int end, int interval) {
- stopScann();
- send(SETSCANANGLE, interval);
- creads = 180/interval;
- reads.clear();
- try {
- countsema.acquire();
- } catch (InterruptedException e1) {
- // TODO Auto-generated catch block
- e1.printStackTrace();
- }
-
- try {
- send(SETSCANANGLE, scannangle);
- countsema.acquire();
- return reads;
- } catch (InterruptedException e) {
- e.printStackTrace();
- }
- return null;
- }
- @Override
- public void scann(RobotReturn r) {
- rr = r;
- send(STARTSCANN);
- }
- @Override
- public void stopScann() {
- send(STOPSCANN);
- }
- @Override
- public void disconnect() {
- send(EXIT);
- if (receivethread == null) return;
- receivethread.run = false;
- try {
- receivethread.join();
- } catch (InterruptedException e1) {
- System.out.println("Nao foi possivel finalizar as threads...");
- }
- try {
- nxtComm.close();
- } catch (IOException e) {
- // TODO Auto-generated catch block
- e.printStackTrace();
- }
-
- }
- @Override
- public void setPose(float x, float y, float a) {
- send(new Pose(x, y, a));
- }
-
-
- @Override
- public String toString() {
- return "Bluetooth Mestre/Escravo";
- }
- }
|