versao funcionando

cmarkers.py

@@ -1,7 +1,6 @@
 import cv2
 import numpy as np
 import math
-from numpy.linalg import norm
 
 validcheck = [[False, True, False, False],
 [False, True, True, False],

lejos/ConsoleTest.java

@@ -9,7 +9,7 @@ public class ConsoleTest {
     public static DataInputStream dataIn;
     public static NXTConnection Link;
 
-    public static int speed = 200, turnSpeed = 100, speedBuffer, speedControl;
+    public static int speed = 200, turnSpeed = 125, speedBuffer, speedControl;
     public static int commandValue,transmitReceived, lastCommand = 0;
 
     public static void main(String [] args) throws Exception {

player-camera.py

@@ -6,13 +6,13 @@ import signal, sys
 import math
 import time
 
-rid = 11
+rid = 12
 run = False
 board = None
 tagplayer = -1
 
 client = TagClient('localhost', 10318)
-client.info(rid, 14, 19, 7, 13.5)
+client.info(rid, 10, 10, 5, 5)
 
 px, py, course = None, None, None
 

player-lejos.py

@@ -5,6 +5,7 @@ from random import randint, random
 import signal, sys
 import math
 import time
+import numpy as np
 
 serverMACAddress = '00:16:53:18:2E:17'
 port = 1
@@ -25,7 +26,7 @@ board = None
 tagplayer = -1
 
 client = TagClient('localhost', 10318)
-client.info(rid, 8, 8, 4, 4)
+client.info(rid, 14, 19, 7, 13.5)
 
 px, py, course = None, None, None
 
@@ -39,7 +40,7 @@ signal.signal(signal.SIGINT, signal_handler)
 players = {}
 
 while not client.quit:
-    time.sleep(0.1)
+    time.sleep(0.4)
 
     # nao recebeu informacoes do campo
     if client.getBoard() is None:
@@ -52,6 +53,7 @@ while not client.quit:
     # jogo esta pausado
     if client.getStatus() is False:
         players = {}
+        s.send(bytes([0x65]))
         continue
 
     # nao recebeu informacoes do pegador
@@ -70,56 +72,36 @@ while not client.quit:
     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]))
+    x1, y1 = 0, 0
+    if time.time() >= interval and len(pos) >= 1 and tag == rid:
+        print("Pegando")
+        x1 = -py+pos[0][1]
+        y1 = -px+pos[0][0]
+    elif tag != rid:
+        print("Fugindo")
+        x1 = py-players[tag][1]
+        y1 = px-players[tag][0]
 
+    else:
+        print("Esperando")
+        s.send(bytes([5]))
+        continue
+    x2 = math.cos(course)
+    y2 = math.sin(course)
+
+    u = np.array([-x1, y1])
+    v = np.array([x2, -y2])
+    angle = np.math.atan2(np.linalg.det([u,v]),np.dot(u,v))
+
+    print([u,v], angle)
 
+    if abs(angle) < 0.2:
+        s.send(bytes([1]))
+    elif angle < 0:
+        s.send(bytes([4]))
     else:
+        s.send(bytes([3]))
 
-        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]))
+s.send(bytes([0x65]))
+s.send(bytes([0x64]))
 

player-virtual.py

@@ -14,7 +14,7 @@ board = None
 tagplayer = -1
 
 client = TagClient('localhost', 10318)
-client.info(rid, 8, 8, 4, 4)
+client.info(rid, 14, 19, 7, 13.5)
 
 px, py, course = None, None, None
 

server.py

@@ -189,9 +189,8 @@ if __name__ == "__main__":
     # Calculate the perspective transform matrix
     transform = None
     if manual and camera:
-        #transform = get_transform_matrix(dev, board_size, dpi)
-        transform = get_transform_matrix_points([(74, 20), (76, 399), (445, 4), (530, 368)]
-, board_size, dpi)
+        transform = get_transform_matrix(dev, board_size, dpi)
+        #transform = get_transform_matrix_points([(74, 20), (76, 399), (445, 4), (530, 368)], board_size, dpi)
 
     server = tag.TagServer(10318, board_size)
 

tag.py

@@ -171,7 +171,10 @@ class TagThread(threading.Thread):
 
     def send(self, msg):
         self.mutex.acquire()
-        self.sock.sendall(msg.encode('ascii'))
+        try:
+            self.sock.sendall(msg.encode('ascii'))
+        except SocketError as e:
+            pass
         self.mutex.release()
 
     def stop(self):