123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339 |
- 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 readm;
- private Semaphore sendm;
-
- private int lastcmd = -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;
-
- public static final byte TYPE_INT = 1;
- public static final byte TYPE_CMD = 2;
- public static final byte TYPE_POSE = 3;
- public static final byte TYPE_FLOAT = 4;
-
- private static final int scannangle = 5;
- private DataOutputStream output;
- private DataInputStream input;
- private Receiver receivethread;
- private Sender sendthread;
-
- private ArrayList<DataRead> reads = null;
- private ArrayList<SendData> tosend;
-
- 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 distance = input.readFloat();
- System.out.println(distance);
- DataRead d = new DataRead();
- d.setDistance(distance);
- d.setSensorAngle(-angle);
- if (rr != null)
- rr.robotData(d);
-
- if (reads != null) {
- readm.tryAcquire();
- reads.add(d);
- readm.release();
- }
-
- } catch (IOException e1) {
- continue;
- }
- }
- }
- }
- }
- private class Sender extends Thread {
- public boolean run = true;
- @Override
- public void run() {
- while(run) {
- SendData value = null;
- if (sendm.tryAcquire()) {
- if (tosend.size() > 0) {
- value = tosend.get(0);
- tosend.remove(0);
- }
- sendm.release();
- }
- if (value != null)
- value.send(output);
- else
- try {
- Thread.sleep(100);
- } catch (InterruptedException e) {
- }
- }
- }
- }
-
- private class SendData {
- float f;
- int i;
- int cmd;
- Pose p;
- int type;
- SendData(int cmd) {
- this.cmd = cmd;
- this.type = TYPE_CMD;
- }
- SendData(int cmd, int i) {
- this.cmd = cmd;
- this.i = i;
- this.type = TYPE_INT;
- }
- SendData(int cmd, float f) {
- this.cmd = cmd;
- this.f = f;
- this.type = TYPE_FLOAT;
- }
- SendData(int cmd, Pose p) {
- this.cmd = cmd;
- this.p = p;
- this.type = TYPE_POSE;
- }
- public boolean send(DataOutputStream output) {
- try {
- switch (type) {
- case TYPE_CMD:
- output.write(cmd);
- break;
- case TYPE_INT:
- output.write(cmd);
- output.write(i);
- break;
- case TYPE_FLOAT:
- output.write(cmd);
- output.writeFloat(f);
- break;
- case TYPE_POSE:
- output.write(cmd);
- output.writeFloat(p.getX());
- output.writeFloat(p.getY());
- output.writeFloat(p.getHeading());
- break;
- default:
- return false;
- }
- output.flush();
- } catch (IOException e) {
- return false;
- }
- return true;
- }
- }
-
- public BluetoothRobot (String name) {
-
- readm = new Semaphore(1);
-
- sendm = new Semaphore(1);
- tosend = new ArrayList<SendData>();
-
- receivethread = new Receiver();
- sendthread = new Sender();
-
- this.name = name;
- }
-
- public void send (SendData sd) {
- if (sendm.tryAcquire()) {
- tosend.add(sd);
- sendm.release();
- }
- }
- @Override
- public void moveForward() {
- if (lastcmd == FORWARD) return;
- send(new SendData(FORWARD));
- lastcmd = FORWARD;
- }
- @Override
- public void moveLeft() {
- if (lastcmd == LEFT) return;
- send(new SendData(LEFT));
- lastcmd = LEFT;
- }
- @Override
- public void moveRight() {
- if (lastcmd == RIGHT) return;
- send(new SendData(RIGHT));
- lastcmd = RIGHT;
- }
- @Override
- public void moveBackward() {
- if (lastcmd == BACKWARD) return;
- send(new SendData(BACKWARD));
- lastcmd = 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(new SendData(SETSCANANGLE, scannangle)); // vai scanear em 5 em 5 graus
- } catch (NXTCommException e) {
- return false;
- }
-
- receivethread.start();
- sendthread.start();
-
- return true;
- }
- @Override
- public void stop() {
- send(new SendData(STOP));
- lastcmd = -1;
- }
- @Override
- public void move(double x) {
- send(new SendData(MOVE, (float)x));
- }
- @Override
- public void rotate(double x) {
- send(new SendData(ROTATE, (float)-x));
- }
- @Override
- public ArrayList<DataRead> scann(int ini, int end, int interval) {
- int readsn = Math.abs(ini-end)/interval;
- send(new SendData(SETSCANANGLE, interval));
- ArrayList<DataRead> tmp;
- reads = new ArrayList<DataRead>();
- scann(null);
- while(true) {
- readm.tryAcquire();
- if (reads.size() >= readsn+1) break;
- readm.release();
- try {
- Thread.sleep(50);
- } catch (InterruptedException e) {
- }
- }
- while (readsn+1 < reads.size() ) {
- reads.remove(reads.size()-1);
- }
- tmp = reads;
- reads = null;
- readm.release();
- stopScann();
- send(new SendData(SETSCANANGLE, scannangle));
- return tmp;
- }
- @Override
- public void scann(RobotReturn r) {
- rr = r;
- send(new SendData(STARTSCANN));
- }
- @Override
- public void stopScann() {
- send(new SendData(STOPSCANN));
- }
- @Override
- public void disconnect() {
- send(new SendData(EXIT));
- if (receivethread == null) return;
- receivethread.run = false;
- try {
- receivethread.join();
- } catch (InterruptedException e1) {
- System.out.println("Nao foi possivel finalizar as threads...");
- }
- if (sendthread == null) return;
- sendthread.run = false;
- try {
- sendthread.join();
- } catch (InterruptedException e1) {
- System.out.println("Nao foi possivel finalizar as threads...");
- }
-
- try {
- nxtComm.close();
- } catch (IOException e) {
- }
- }
- @Override
- public void setPose(float x, float y, float a) {
- send(new SendData(SETPOSE, new Pose(-y, x, a)));
- }
-
-
- @Override
- public String toString() {
- return "Bluetooth Mestre/Escravo";
- }
- }
|