diff --git a/robot_utils.py b/robot_utils.py index cbf16efb8c5fa8d441c7adf24b3e7a5d65563e0f..fc11a2b05cc37d648df5ba7f83c20fdb759025a6 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() + +