From a4b0ff64002e588e275792110a5eebbd944ab8b2 Mon Sep 17 00:00:00 2001
From: Lukas Hoffleit <lukas.hoffleit03@gmail.com>
Date: Thu, 10 Apr 2025 19:13:20 +0200
Subject: [PATCH] Pick-Place-Methode; TODO: Greiferoeffnung

---
 robot_utils.py | 31 +++++++++++++++++++++++++++++++
 1 file changed, 31 insertions(+)
 create mode 100644 robot_utils.py

diff --git a/robot_utils.py b/robot_utils.py
new file mode 100644
index 0000000..cbf16ef
--- /dev/null
+++ b/robot_utils.py
@@ -0,0 +1,31 @@
+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()
+
-- 
GitLab