diff --git a/robot_utils.py b/robot_utils.py new file mode 100644 index 0000000000000000000000000000000000000000..cbf16efb8c5fa8d441c7adf24b3e7a5d65563e0f --- /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() +