diff --git a/ur5_control.py b/ur5_control.py
index 084a6ed..d655f70 100755
--- a/ur5_control.py
+++ b/ur5_control.py
@@ -10,7 +10,7 @@ import yaml
 from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
 import sys
 from util import fprint
-
+from pyModbusTCP.client import ModbusClient
 
 
 
@@ -436,27 +436,49 @@ def pick_up_routine(robot, holder_index, verbose=False):
         print('Pickup routine for index', holder_index)
 
     goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
+    open_gripper()
+
     curr_pos = rob.getl()
     new_pos = curr_pos
     new_pos[2] = 0.005
     rob.movel(new_pos, vel=0.1, acc=1)
     # goto_holder_index(robot, holder_index, 0.0)
+    close_gripper()
+    
 
-    # Close Gripper code
-    time.sleep(0.5)
 
     new_pos[2] = 0.2
     rob.movel(new_pos, vel=0.1, acc=1)
     was_flipped = is_flipped(robot)
 
     # # Tray position 1
-    # rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2)
-    # rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2)
-    # time.sleep(0.5)
+    rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2)
+    rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2)
+    time.sleep(0.5)
+    open_gripper()
 
     # Back to safe position
-    # rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2)
+    rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2)
 
+def open_gripper():
+    print("opening")
+    c = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=False)
+    
+    c.write_single_register(0, 0b0)
+    c.write_single_register(435, 0b1)
+    time.sleep(0.5)
+    c.close()
+    
+    #c.close()
+
+def close_gripper():
+    print("closing")
+    c = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=False)
+    c.write_single_register(435, 0b0)
+    c.write_single_register(0, 0b1)
+    time.sleep(0.5)
+    c.close()
+    #
 
 if __name__ == "__main__":
     
@@ -466,6 +488,15 @@ if __name__ == "__main__":
     with open('config.yml', 'r') as fileread:
         #global config
         config = yaml.safe_load(fileread)
+    
+    open_gripper()
+    #time.sleep(5)
+    #close_gripper()
+    
+    #time.sleep(100)
+    
+    
+    
     robot = Rob(config) # robot of type Rob is the custom class above
     robot.init_arm()
     rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously
@@ -514,7 +545,7 @@ if __name__ == "__main__":
     # rob.movej(get_joints_from_xyz_abs(robot, 0.2, 0, 0.05), vel=0.5, acc=2)
     # goto_holder_index(robot, 27, 0.05)
 
-    for i in [1,50]:
+    for i in [6,7,8]:
         pick_up_routine(robot, i, verbose=True)