diff --git a/config.yml b/config.yml
index fcaccd8..3c69c9a 100644
--- a/config.yml
+++ b/config.yml
@@ -511,7 +511,7 @@ position_map:
   - index: 36
     pos: [-228.6, -131.982]
   - index: 37
-    pos: [-152.4, -131.982]
+    pos: [-151, -133]
   - index: 38
     pos: [-76.2, -131.982]
   - index: 39
@@ -527,7 +527,7 @@ position_map:
   - index: 44
     pos: [-114.3, -197.973]
   - index: 45
-    pos: [-38.1, -197.973]
+    pos: [-35, -199]
   - index: 46
     pos: [38.1, -197.973]
   - index: 47
diff --git a/ur5_control.py b/ur5_control.py
index b8b56f3..7d440fe 100755
--- a/ur5_control.py
+++ b/ur5_control.py
@@ -486,7 +486,7 @@ def safe_move(robot, x, y, z, use_closest_path=True):
     if (r <= flip_radius and is_flipped(robot)) or (r > flip_radius and not is_flipped(robot)):
         flip(robot)
 
-    rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot), use_closest_path=use_closest_path), vel=4, acc=3)
+    rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot), use_closest_path=use_closest_path), vel=2, acc=2)
     return True
 
 def holder_routine(robot, holder_index, pick_up, verbose=False):
@@ -711,15 +711,25 @@ if __name__ == "__main__":
 
     #move_to_packup(robot)
     
+    # pick_up_holder(robot, 2)
+    # drop_off_tray(robot, 0)
 
-    # for i in range(3):
-    #     pick_up_holder(robot, i)
+    # pick_up_tray(robot, 1)
+    # drop_off_holder(robot, 5)
 
-    #     print('Drop off', i+1)
-    #     drop_off_holder(robot, i+1)
+    # pick_up_holder(robot, 26)
+    # drop_off_tray(robot, 3)
 
-    holder_to_camera(robot, 0)
-    camera_to_holder(robot, 0)
+
+    for i in range(44,45):
+        pick_up_holder(robot, i)
+
+        print('Drop off', i+1)
+        drop_off_holder(robot, i+1)
+        input()
+
+    # holder_to_camera(robot, 0)
+    # camera_to_holder(robot, 0)
 
     print("Current tool pose is: ",  rob.getl())
     curr_pos = rob.getl()