diff --git a/ur5_control.py b/ur5_control.py index cf9c683..189accf 100755 --- a/ur5_control.py +++ b/ur5_control.py @@ -681,8 +681,11 @@ def camera_to_holder(robot, pos_updates, holder_index, verbose=False): def open_gripper(): fprint("Opening gripper") - c = ModbusClient(host="192.168.1.21", port=502, auto_open=True, auto_close=False) - + c = ModbusClient(host="192.168.1.21", port=502, auto_open=False, auto_close=False) + c.open() + while not c.is_open(): + time.sleep(0.01) + c.write_single_register(112, 0b0) c.write_single_register(435, 0b10000000) time.sleep(0.5) @@ -695,7 +698,11 @@ def open_gripper(): def close_gripper(): fprint("Closing gripper") - c = ModbusClient(host="192.168.1.21", port=502, auto_open=True, auto_close=False) + c = ModbusClient(host="192.168.1.21", port=502, auto_open=False, auto_close=False) + c.open() + while not c.is_open(): + time.sleep(0.01) + c.write_single_register(435, 0b00000000) c.write_single_register(112, 0b1) time.sleep(0.5) @@ -703,6 +710,7 @@ def close_gripper(): c.write_single_register(112, 0b1) time.sleep(0.5) c.close() + time.sleep(0.2) # def get_position_thread(robot, pos_updates):