make functions for open/close gripper

Author: hganchev

main.py

@@ -42,20 +42,26 @@ def PickAndPlace():
     ur_control.move_joint_with_pose(list(map(add,p2_pick,p2_offset_approach)), a=1.4, v=1.05, t=0, r=0) # move to p2_pick with offset for approach
     ur_control.move_linear_pose(p2_pick, a=1.4, v=1.05, t=0, r=0) # move to p2_pick with linear motion
     # pick the piece - close gripper
-    ur_control.set_digital_output(0, True)
-    ur_control.set_digital_output(1, False)
+    CloseGripper()
     ##
     ur_control.move_joint_with_pose(list(map(add,p2_pick,p2_offset_leave)), a=1.4, v=1.05, t=0, r=0) # move to p2_pick with offset to leave
 
     # Place
     ur_control.move_joint_with_pose(list(map(add,p3_place,p3_offset_approach)), a=1.4, v=1.05, t=0, r=0) # move to p3_place with offset for approach
     ur_control.move_linear_pose(p3_place, a=1.4, v=1.05, t=0, r=0) # move to p3_place with linear motion
     # place the piece - open gripper
-    ur_control.set_digital_output(0, False)
-    ur_control.set_digital_output(1, True)
+    OpenGripper()
     ##
     ur_control.move_joint_with_pose(list(map(add,p3_place,p3_offset_leave)), a=1.4, v=1.05, t=0, r=0) # move to p3_place with offset to leave
 
+def OpenGripper():
+    ur_control.set_digital_output(0, False)
+    ur_control.set_digital_output(4, True)
+
+def CloseGripper():
+    ur_control.set_digital_output(0, True)
+    ur_control.set_digital_output(4, False)
+
 
 if __name__ == '__main__':
     program()