米厂码垛修改

Model/Position.py

@@ -2,6 +2,7 @@ import math
 
 from Constant import position_accuracy_command
 from Constant import position_accuracy_action
+from Constant import DebugPosition
 class Position:
     def __init__(self):
         self.X = 0.0
@@ -10,18 +11,36 @@ class Position:
         self.U = 0.0
         self.V = 0.0
         self.W = 0.0
+        self.Axis_0 = 0.0
+        self.Axis_1 = 0.0
+        self.Axis_2 = 0.0
+        self.Axis_3 = 0.0
+        self.Axis_4 = 0.0
+        self.Axis_5 = 0.0
+        #点位类型 1世界坐标(默认）4关节坐标
+        self.position_type =1
+        
         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)
+         if DebugPosition:
+            return True
+         #点位类型 1世界坐标(默认）4关节坐标
+         if(position.position_type==4):
+             return self._compare_joint(position,is_action)
+         else:
+             return self._compare_world(position,is_action)
+
+    def _compare_world(self,position,is_action=False):
+        """
+        世界坐标比较
+        :param position:
+        :return:精度内TRUE,否则为FALSE
+        """
+      
         distance = math.sqrt((self.X - position.X) ** 2 +
                              (self.Y - position.Y) ** 2 +
                              (self.Z - position.Z) ** 2 )
@@ -30,6 +49,20 @@ class Position:
         else:
             return False
 
+    def _compare_joint(self,position,is_action=False):
+        """
+        关节坐标比较
+        :param position:
+        :return:精度内TRUE,否则为FALSE
+        """
+        distance = math.sqrt((self.Axis_0 - position.X) ** 2 +
+                             (self.Axis_1 - position.Y) ** 2 +
+                             (self.Axis_2 - position.Z) ** 2 + 
+                             (self.Axis_3 - position.U) ** 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 \
@@ -83,6 +116,16 @@ class Real_Position(Position):
         self.W = W
         return self
 
+    def init_position_joint_and_world(self, X, Y, Z, U, V, W,Axis_0,Axis_1,Axis_2,Axis_3,Axis_4,Axis_5):
+        self.init_position(X,Y,Z,U,V,W) 
+        self.Axis_0 = Axis_0
+        self.Axis_1 = Axis_1
+        self.Axis_2 = Axis_2
+        self.Axis_3 = Axis_3
+        self.Axis_4 = Axis_4
+        self.Axis_5 = Axis_5
+        return self
+
     # def init_position(self, position):
     #     return self.init_position(position.X,position.Y,position.Z,position.U,position.V,position.W)