From ac0817b4daa6d42997c331fe50c38eb94305243b Mon Sep 17 00:00:00 2001
From: Lukas Hoffleit <lukas.hoffleit03@gmail.com>
Date: Thu, 8 May 2025 17:17:43 +0200
Subject: [PATCH] Pick-Place fertig
---
robot_utils.py | 51 ++++++++++++++++++++++++++++++++++++++------------
1 file changed, 39 insertions(+), 12 deletions(-)
diff --git a/robot_utils.py b/robot_utils.py
index cbf16ef..fc11a2b 100644
--- a/robot_utils.py
+++ b/robot_utils.py
@@ -1,31 +1,58 @@
import phantomController as pc
import time
-PICK_HIGHT = 10
-LIFT_HIGHT = 50
+import traceback
+PICK_HIGHT = 15
+LIFT_HIGHT = 100
+GRIP_OPEN = 511
+GRIP_CLOSED = 400
+
+class RobotError(Exception):
+ """Error during Robot Move"""
class RobotUtils():
+ """ Class to abstract the robot driver from the user """
+
def __init__(self) -> None:
self.ser = pc.connect()
-
+
+ def move_to_pose(self, tcv, grip, speed) -> None:
+ msg = pc.move_pose(self.ser, tcv, grip, speed, True)
+ if "done" not in msg:
+ print(msg)
+ raise RobotError(f"Invalid move({tcv})")
+
def pick_place(self, start, finish) -> None:
- start.extend([PICK_HIGHT, 0, 0, -1])
- pc.move_pose(self.ser, start, 0, 2000)
+ print("A")
+ start.extend([LIFT_HIGHT, 0, 0, -1])
+ self.move_to_pose(start, GRIP_OPEN, 1000)
+ print("B")
+ start[2] = PICK_HIGHT
+ self.move_to_pose(start, GRIP_OPEN, 1000)
+ print("C")
+ self.move_to_pose(start, GRIP_CLOSED, 1000)
start[2] = LIFT_HIGHT
- pc.move_pose(self.ser, start, 0, 2000)
+ self.move_to_pose(start, GRIP_CLOSED, 1000)
finish.extend([LIFT_HIGHT, 0, 0, -1])
- pc.move_pose(self.ser, finish, 0, 2000)
+ self.move_to_pose(finish, GRIP_CLOSED, 1000)
finish[2] = PICK_HIGHT
- pc.move_pose(self.ser, finish, 0, 2000)
+ self.move_to_pose(finish, GRIP_CLOSED, 1000)
+ self.move_to_pose(finish, GRIP_OPEN, 1000)
+ finish[2] = LIFT_HIGHT
+ self.move_to_pose(finish, GRIP_OPEN, 1000)
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()
+ try:
+ robot.pick_place([130, 120], [50, 120])
+ robot.disconnect()
+ except:
+ print(traceback.format_exc())
+ robot.disconnect()
+
+
--
GitLab