From 62ec1e74430bf618a9c588526f29f44b45c10f0f Mon Sep 17 00:00:00 2001
From: Cole Deck <cdeck@hawk.iit.edu>
Date: Tue, 14 May 2024 16:01:09 -0500
Subject: [PATCH] Add tray return API call

---
 led_control.py |  3 ++-
 run.py         |  3 +++
 ur5_control.py | 18 +++++++++---------
 3 files changed, 14 insertions(+), 10 deletions(-)

diff --git a/led_control.py b/led_control.py
index 7eeae7e..9c0b98a 100755
--- a/led_control.py
+++ b/led_control.py
@@ -479,7 +479,8 @@ class LEDSystem():
                             #    ratio = 0
                             self.data[idx] = (int(base[0] + ratio * deltar), int(base[1] + ratio * deltag), 100) #base[2] + ratio * deltab)
             else:
-                print("No arm position!")
+                #print("No arm position!")
+                pass
         self.sendall(self.data)
         return self
 
diff --git a/run.py b/run.py
index 935eb0e..a01e276 100755
--- a/run.py
+++ b/run.py
@@ -329,6 +329,9 @@ def check_server():
                             elif "position" in data:
                                 fprint("Adding cable to dispense queue")
                                 mainloop_get.put(("pickup", data["position"]))
+                            elif "tray" in data:
+                                fprint("Adding tray return to dispense queue")
+                                mainloop_get.put(("return", data["tray"]))
                             else:
                                 fprint("Invalid data.")
                     
diff --git a/ur5_control.py b/ur5_control.py
index dddccaa..f4c1c14 100755
--- a/ur5_control.py
+++ b/ur5_control.py
@@ -754,10 +754,10 @@ if __name__ == "__main__":
     rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously
 
 
-    #move_to_packup(robot)
+    move_to_packup(robot)
     
     # pick_up_holder(robot, 2)
-    # drop_off_tray(robot, 0)
+    #drop_off_tray(robot, 0)
     # drop_off_tray(robot, 1)
     # drop_off_tray(robot, 2)
     # drop_off_tray(robot, 3)
@@ -769,15 +769,15 @@ if __name__ == "__main__":
     # drop_off_tray(robot, 3)
 
 
-    for i in range(0,54):
-        pick_up_holder(robot, None, i)
+    # for i in range(0,54):
+    #     pick_up_holder(robot, None, i)
 
-        #print('Drop off', i+1)
-        drop_off_tray(robot, 0)
-        #input()
+    #     #print('Drop off', i+1)
+    #     drop_off_tray(robot, 0)
+    #     #input()
 
-    # holder_to_camera(robot, 0)
-    # camera_to_holder(robot, 0)
+    # # holder_to_camera(robot, 0)
+    # # camera_to_holder(robot, 0)
 
     print("Current tool pose is: ",  rob.getl())
     curr_pos = rob.getl()