Pick-Place fertig

robot_utils.py

 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()
+    try:
         robot.pick_place([130, 120], [50, 120])
         robot.disconnect()
+    except:
+        print(traceback.format_exc())
+        robot.disconnect()
+
+