first commit

Model/Position.py

@@ -0,0 +1,90 @@
+import math
+
+from Constant import position_accuracy_command
+from Constant import position_accuracy_action
+class Position:
+    def __init__(self):
+        self.X = 0.0
+        self.Y = 0.0
+        self.Z = 0.0
+        self.U = 0.0
+        self.V = 0.0
+        self.W = 0.0
+        self.a = 0.0
+        self.b = 0.0
+        self.c = 0.0
+
+
+    def compare(self,position,is_action=False):
+        # distance = math.sqrt((self.X-position.X)**2+
+        #                      (self.Y-position.Y)**2+
+        #                      (self.Z - position.Z)**2+
+        #                      (self.U - position.U)**2+
+        #                      (self.V - position.V)**2+
+        #                      (self.W - position.W) ** 2)
+        distance = math.sqrt((self.X - position.X) ** 2 +
+                             (self.Y - position.Y) ** 2 +
+                             (self.Z - position.Z) ** 2 )
+        if distance<=(position_accuracy_action if is_action else position_accuracy_command):
+            return True
+        else:
+            return False
+
+
+    # def compare(self,position):
+    #     if self.X-position.X<position_accuracy and \
+    #         self.Y-position.Y<position_accuracy and \
+    #         self.Z - position.Z < position_accuracy and \
+    #         self.U - position.U < position_accuracy and \
+    #         self.V - position.V < position_accuracy and \
+    #         self.W - position.W < position_accuracy:
+    #         return True
+    #     else:
+    #         return False
+    #     pass
+
+    def is_error_angel_move(self,position,interval):
+        if self.X - position.X >= interval or \
+                self.Y - position.Y >= interval or \
+                self.Z - position.Z >= interval or \
+                self.U - position.U >= interval or \
+                self.V - position.V >= interval or \
+                self.W - position.W >= interval:
+            return True
+        else:
+            return False
+        pass
+"""
+摄像头识别位置和角度
+"""
+class Detection_Position(Position):
+    def __init__(self):
+        super().__init__()
+
+    def init_position(self,X,Y,Z,U,V,W):
+        self.X = X
+        self.Y = Y
+        self.Z = Z
+        self.U = U
+        self.V = V
+        self.W = W
+
+
+class Real_Position(Position):
+    def __init__(self):
+        super().__init__()
+
+    def init_position(self, X, Y, Z, U, V, W):
+        self.X = X
+        self.Y = Y
+        self.Z = Z
+        self.U = U
+        self.V = V
+        self.W = W
+        return self
+
+    # def init_position(self, position):
+    #     return self.init_position(position.X,position.Y,position.Z,position.U,position.V,position.W)
+
+    def to_string(self):
+        return "X:{:.3f},Y:{:.3f},Z:{:.3f},U:{:.3f},V:{:.3f},W:{:.3f}".format(self.X,self.Y,self.Z,self.U,self.V,self.W)