From f4e43f33d289b450d954dc684646078d2a01fa49 Mon Sep 17 00:00:00 2001
From: BlueOceanWave <97416032+BlueOceanWave@users.noreply.github.com>
Date: Wed, 24 Apr 2024 14:40:38 -0500
Subject: [PATCH] Created functions for routines

---
 ur5_control.py | 77 ++++++++++++++++++++------------------------------
 1 file changed, 31 insertions(+), 46 deletions(-)

diff --git a/ur5_control.py b/ur5_control.py
index b25fc97..fd83af0 100755
--- a/ur5_control.py
+++ b/ur5_control.py
@@ -493,6 +493,10 @@ def holder_routine(robot, holder_index, pick_up, verbose=False):
     robot = connect(robot)
     rob = robot.robot
     
+    # Don't attempt to place a tube in the camera slot
+    if holder_index == 49:
+        return
+
     if verbose:
         fprint('Pickup routine for index' + str(holder_index))
 
@@ -532,6 +536,10 @@ def tray_routine(robot, slot=0, pick_up=True):
     robot = connect(robot)
     rob = robot.robot
 
+    # Default to 0 if invalid value
+    if slot not in [0, 1, 2, 3]:
+        slot = 0
+
     slot_prepositions =  [(-9.93, -112.67, 144.02, -116.69, -54.13, -10.29),
                          (-12.35, -124.95, 148.61, -107.27, -54.36, -13.26),
                          (-16.45, -96.97, 137.85, 58.39, -305.08, 161.75),
@@ -632,50 +640,30 @@ def return_routine(robot, slot, holder_index=None, verbose=False):
         rob.movej(offset_gripper_angle(robot, 0.35, -0.35, 0.3, flip=was_flipped, use_closest_path=False), vel=2, acc=2)
         return True
     
-    
-def to_camera(robot, holder_index, verbose=False):
+
+def goto_camera(robot):
+    robot = connect(robot)
+    goto_holder_index(robot, 49, 0.2)
+
+def tray_to_camera(robot, slot):
+    pick_up_tray(robot, slot)
+    goto_camera(robot)
+def holder_to_tray(robot, holder_index, slot):
+    pick_up_holder(robot, holder_index)
+    drop_off_tray(robot, slot)
+
+def holder_to_camera(robot, holder_index, verbose=False):
     robot = connect(robot)
     fprint("Bringing tube at " + str(holder_index) + " to camera")
     rob = robot.robot
 
-    goto_holder_index(robot, holder_index, 0.05, 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()
-    
-
-
-    new_pos[2] = 0.2
-    rob.movel(new_pos, vel=2, acc=1)
-    was_flipped = is_flipped(robot)
-
-    # start inwards
-    rob.movej(offset_gripper_angle(robot, 0.15, -0.1, 0.25, flip=was_flipped, use_closest_path=False), vel=2, acc=2)
-    
-    # go to camera
-    rob.movej(offset_gripper_angle(robot, 0.3, -0.2, 0.25, flip=was_flipped, use_closest_path=False), vel=2, acc=2)
-    return True
-
-def return_camera(robot, holder_index, verbose=False):
+    pick_up_holder(robot, holder_index)
+    goto_camera(robot)
+def camera_to_holder(robot, holder_index, verbose=False):
     robot = connect(robot)
     rob = robot.robot
 
-    # move inwards first
-    rob.movej(offset_gripper_angle(robot, 0.15, -0.1, 0.25, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2)
-    goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
-    curr_pos = rob.getl()
-    new_pos = curr_pos
-    new_pos[2] = 0.015
-    rob.movel(new_pos, vel=0.1, acc=1)
-    open_gripper()
-    new_pos[2] = 0.1
-    rob.movel(new_pos, vel=2, acc=1)
+    drop_off_holder(robot, holder_index)
     
 
 def open_gripper():
@@ -724,17 +712,14 @@ if __name__ == "__main__":
     #move_to_packup(robot)
     
 
-    pick_up_holder(robot, 0)
-    drop_off_tray(robot, 0)
-    pick_up_holder(robot, 1)
-    drop_off_tray(robot, 3)
-
-    pick_up_tray(robot, 0)
-    drop_off_holder(robot, 0)
-    pick_up_tray(robot, 3)
-    drop_off_holder(robot, 1)
+    # for i in range(3):
+    #     pick_up_holder(robot, i)
 
+    #     print('Drop off', i+1)
+    #     drop_off_holder(robot, i+1)
 
+    holder_to_tray(robot, 0, 1)
+    tray_to_camera(robot, 1)
    
     print("Current tool pose is: ",  rob.getl())
     curr_pos = rob.getl()