Pick-Place-Methode; TODO: Greiferoeffnung

robot_utils.py

+import phantomController as pc
+import time
+PICK_HIGHT = 10
+LIFT_HIGHT = 50
+
+class RobotUtils():
+    def __init__(self) -> None:
+        self.ser = pc.connect()
+    
+    def pick_place(self, start, finish) -> None:
+        start.extend([PICK_HIGHT, 0, 0, -1])
+        pc.move_pose(self.ser, start, 0, 2000)
+        start[2] = LIFT_HIGHT
+        pc.move_pose(self.ser, start, 0, 2000)
+        finish.extend([LIFT_HIGHT, 0, 0, -1])
+        pc.move_pose(self.ser, finish, 0, 2000)
+        finish[2] = PICK_HIGHT 
+        pc.move_pose(self.ser, finish, 0, 2000)
+
+    def disconnect(self) -> None:
+        pc.sleep(self.ser)
+        time.sleep(0.1)
+        pc.disconnect(self.ser)
+
+
+
+if __name__ == "__main__":
+    robot = RobotUtils()
+    robot.pick_place([130, 120], [50, 120])
+    robot.disconnect()
+