Add shutdown routine

run.py

@@ -338,6 +338,9 @@ def check_server():
                         elif call == 'request':
                             if "position" in data:
                                 mainloop_get.put(("show", data["position"]))
+                    
+                    case "shutdown":
+                        mainloop_get.put(("shutdown", 0))
                     case _:
                         fprint("Unknown/unimplemented data type: " + decoded["type"])
             except Exception as e:
@@ -571,7 +574,7 @@ def mainloop_server(pool, manager):
             print(ring_animation, animation_wait, ledsys.mode, arm_position)
             
 
-    if start_animation:
+    if start_animation and real:
         # animation start requested
         # may not be immediate
         if ring_animation is not None:
@@ -601,7 +604,7 @@ def mainloop_server(pool, manager):
     
     else:
         # no new animation
-        if ring_animation is not None:
+        if ring_animation is not None and real:
             ledsys.mainloop(None, ring_animation, arm_position=arm_position)
             
         else:
@@ -790,6 +793,12 @@ def mainloop_server(pool, manager):
                     ring_animation = get_cable
                     start_animation = True
                     led_set_mode = "Show"
+                    fprint("Showing cable at position " + str(get_cable))
+                elif action == "shutdown":
+                    fprint("SHUTTING DOWN!!")
+                    pool.apply_async(ur5_control.move_to_packup, (arm,), callback=arm_start_callback, error_callback=handle_error)
+                    sleep(30)
+                    killme.set(1)
                 else:
                     fprint("Movement requested. Keep clear of the machine!")
 

ur5_control.py

@@ -266,6 +266,7 @@ def move_to_home(robot, keep_flip=False, speed=2):
     return True
 
 def move_to_packup(robot, speed=0.25):
+    robot = connect(robot)
     rob = robot.robot
     
     # known good starting point to reach store position