errado

cmarkers.py

@@ -8,15 +8,15 @@ def getCMarkers (img):
     gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
     img3 = cv2.adaptiveThreshold(gray, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 125, 10)
 
-    kernel = np.ones((6,6),np.uint8)
+    kernel = np.ones((3,3),np.uint8)
     img2 = cv2.morphologyEx(img3, cv2.MORPH_CLOSE, kernel)
 
     # kernel = cv2.getStructuringElement(cv2.MORPH_CROSS,(3,3))
     # img2 = cv2.dilate(img2,kernel,iterations = 2)
 
     kernel = np.ones((3,3),np.uint8)
-    img2 = cv2.dilate(img2,kernel, 3)
-    # cv2.imshow("m2", img2)
+    img2 = cv2.dilate(img2,kernel, 2)
+    cv2.imshow("m2", img2)
 
     # Setup SimpleBlobDetector parameters.
     params = cv2.SimpleBlobDetector_Params()
@@ -168,4 +168,4 @@ def get_transform_matrix_points(corners, board_size, dpi):
         # If we were given a calibration filename, save this matrix to a file
         return trans
     else:
-        return None
+        return None

lejos/ConsoleTest.java

@@ -0,0 +1,80 @@
+import lejos.nxt.Motor;
+import java.io.*;
+import lejos.nxt.comm.*;
+import lejos.util.Delay;
+import lejos.nxt.LCD;
+
+public class ConsoleTest {
+    public static DataOutputStream dataOut;
+    public static DataInputStream dataIn;
+    public static NXTConnection Link;
+
+    public static int speed = 200, turnSpeed = 100, speedBuffer, speedControl;
+    public static int commandValue,transmitReceived, lastCommand = 0;
+
+    public static void main(String [] args) throws Exception {
+        connect();
+
+        while(true){
+            if (dataIn.available() > 0){
+                byte x = dataIn.readByte();
+                if(checkCommand((int)x)) {
+                    disconnect();
+                    break;
+                }
+            }
+        }
+    }
+ 
+    public static boolean checkCommand(int data) {
+        System.out.println(data);
+        switch (data) {
+            case 0x64:
+                return true;
+            case 0x1: // forward
+                Motor.A.setSpeed(speed);
+                Motor.C.setSpeed(speed);
+                Motor.A.forward(); 
+                Motor.C.forward();
+                break;
+            case 0x2: // forward
+                Motor.A.setSpeed(speed);
+                Motor.C.setSpeed(speed);
+                Motor.A.backward(); 
+                Motor.C.backward();
+                break;
+            case 0x3: // forward
+                Motor.A.setSpeed(speed);
+                Motor.C.setSpeed(speed);
+                Motor.A.forward(); 
+                Motor.C.backward();
+                break;
+            case 0x4: // forward
+                Motor.A.setSpeed(speed);
+                Motor.C.setSpeed(speed);
+                Motor.A.backward(); 
+                Motor.C.forward();
+                break;
+           default:
+                Motor.A.stop(true); 
+                Motor.C.stop();
+        }
+        return false;
+    }
+
+    public static void connect() {  
+        System.out.println("Listening..");
+        Link = Bluetooth.waitForConnection(0, NXTConnection.RAW);
+        dataOut = Link.openDataOutputStream();
+        dataIn = Link.openDataInputStream();
+
+    }
+
+    public static void disconnect() throws java.io.IOException {  
+        System.out.println("Closing...");
+        dataOut.close();
+        dataIn.close();
+        USB.usbReset();
+        System.exit(0);
+    }
+}

player-lejos.py

@@ -1,18 +1,123 @@
 import bluetooth
-# pybluez
-nearby_devices = bluetooth.discover_devices(lookup_names=True)
-print("found %d devices" % len(nearby_devices))
+from robot import Robot
+from tag import TagClient
+from random import randint, random
+import signal, sys
+import math
+import time
 
-for addr, name in nearby_devices:
-    print("  %s - %s" % (addr, name))
-
-serverMACAddress = '00:16:53:17:ea:9d'
-port = 3
+serverMACAddress = '00:16:53:18:2E:17'
+port = 1
 s = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
 s.connect((serverMACAddress, port))
-while 1:
-    text = raw_input() # Note change to the old (Python 2) raw_input
-    if text == "quit":
-        break
-    s.send(text)
-sock.close()
+
+#while 1:
+#    text = bytes([1])
+#    if text == "quit":
+#        break
+#    s.send(bytes([1]))
+#sock.close()
+
+
+rid = randint(100, 1000)
+run = False
+board = None
+tagplayer = -1
+
+client = TagClient('localhost', 10318)
+client.info(rid, 8, 8, 4, 4)
+
+px, py, course = None, None, None
+
+# Sinal de ctrl-c
+def signal_handler(signal, frame):
+    client.stop()
+    sock.close()
+    sys.exit(0)
+signal.signal(signal.SIGINT, signal_handler)
+
+players = {}
+
+while not client.quit:
+    time.sleep(0.1)
+
+    # nao recebeu informacoes do campo
+    if client.getBoard() is None:
+        continue
+    elif px is None:
+        board = client.getBoard()
+        px, py = board[0]*random(), board[1]*random()
+        course = math.pi*2.0*random()
+
+    # jogo esta pausado
+    if client.getStatus() is False:
+        players = {}
+        continue
+
+    # nao recebeu informacoes do pegador
+    if client.getTagInfo() is None:
+        continue
+
+    # verifica se ha posicoes novas
+    newposs = client.getPosition()
+    for info in newposs:
+        posx, posy, course, idd, xsize, ysize, xtag, ytag = info
+        if idd != rid:
+            players[idd] = [posx, posy]
+
+    tag, interval = client.getTagInfo()  
+    pos = [players[p] for p in players]
+
+    if time.time() <= interval:
+        # comportamento de fuga
+        if tag in players and tag != rid:
+            # foge se nao for pegador 
+
+            x1 = px-players[tag][0]
+            y1 = py-players[tag][1]
+            x2 = math.cos(course)
+            y2 = math.sin(course)
+            dot = x1*x2 + y1*y2      # dot product between [x1, y1] and [x2, y2]
+            det = x1*y2 - y1*x2      # determinant
+            angle = math.atan2(det, dot)
+	    if angle < 0.3 or angle > math.pi-0.3:
+                s.send(bytes([1]))
+            elif angle > math.pi:
+                s.send(bytes([3]))
+            else:
+                s.send(bytes([4]))
+
+
+    else:
+
+        if len(pos) >= 1 and tag == rid:
+            x1 = px-pos[0][0]
+            y1 = py-pos[0][1]
+            x2 = math.cos(course)
+            y2 = math.sin(course)
+            dot = x1*x2 + y1*y2      # dot product between [x1, y1] and [x2, y2]
+            det = x1*y2 - y1*x2      # determinant
+            angle = math.atan2(det, dot)
+	    if angle < 0.3 or angle > math.pi-0.3:
+                s.send(bytes([1]))
+            elif angle > math.pi:
+                s.send(bytes([3]))
+            else:
+                s.send(bytes([4]))
+
+        if tag != rid:
+
+            x1 = px-players[tag][0]
+            y1 = py-players[tag][1]
+            x2 = math.cos(course)
+            y2 = math.sin(course)
+            dot = x1*x2 + y1*y2      # dot product between [x1, y1] and [x2, y2]
+            det = x1*y2 - y1*x2      # determinant
+            angle = math.atan2(det, dot)
+	    if angle < 0.3 or angle > math.pi-0.3:
+                s.send(bytes([1]))
+            elif angle > math.pi:
+                s.send(bytes([3]))
+            else:
+                s.send(bytes([4]))
+

server.py

@@ -152,7 +152,7 @@ def get_green_dots(img_orig):
 ### Calibration Example ###
 if __name__ == "__main__":
     parser = argparse.ArgumentParser()
-    cam_id = 0
+    cam_id = 1
     dev = None
     
     parser.add_argument('-a',