Add LED animations to runtime

led_control.py

@@ -221,13 +221,14 @@ class LEDSystem():
                 self.data.append((0,0,0))
         self.sendall(self.data)
         #time.sleep(50000)    
-        fprint("Running start-up test sequence...")
+        # fprint("Running start-up test sequence...")
-        for y in range(1):
+        # for y in range(1):
-            for x in range(len(self.leds)):
+        #     for x in range(len(self.leds)):
-                self.setpixel(0,60,144,x)
+        #         self.setpixel(0,60,144,x)
-            self.sendall(self.data)
+        #     self.sendall(self.data)
-            #time.sleep(2)
+        #     #time.sleep(2)
-        self.alloffsmooth()
+        # self.alloffsmooth()
+        self.startup_animation(show=False)
 
     def sendall(self, datain):
         # send all LED data to all controllers

run.py

@@ -27,8 +27,11 @@ import process_video
 import search
 from search import JukeboxSearch
 #multiprocessing.set_start_method('spawn', True)
+from pyModbusTCP.client import ModbusClient
+from uptime import uptime
 
 
+mbconn = None
 config = None
 keeprunning = True
 arm_ready = False
@@ -45,6 +48,7 @@ ledsys = None
 arm = None
 to_server_queue = Queue()
 from_server_queue = Queue()
+mainloop_get = Queue()
 mode = "Startup"
 counter = 0
 jbs = None
@@ -52,11 +56,17 @@ scan_value = None
 arm_state = None
 cable_list = list()
 parse_res = None
+cable_list_state = list()
+just_placed = -1
+ring_animation = None
+led_set_mode = None
 
 def arm_start_callback(res):
+    fprint("Arm action complete.")
     global arm_ready
     arm_ready = True
     
+
 def led_start_callback(res):
     global led_ready
     led_ready = True
@@ -229,6 +239,19 @@ def start_server_socket(cable_list):
                             pass
                         elif call == "request":
                             pass
+
+                    case "cable_get":
+                        fprint("cable_get message")
+                        if call == "send":
+                            global mainloop_get
+                            if "part_number" in data:
+                                for cableidx in range(len(cable_list)):
+                                    cable = cable_list[cableidx]
+                                    if cable == data["part_number"]:
+                                        mainloop_get.put(("pickup", cableidx))
+                            elif "position" in data:
+                                mainloop_get.put(("pickup", data["position"]))
+                    
                     case _:
                         fprint("Unknown/unimplemented data type: " + decoded["type"])
             except Exception as e:
@@ -285,11 +308,12 @@ def setup_server(pool):
     global camera
     global arm
     global jbs
+
     arm = Rob(config)
-    pool.apply_async(arm.init_arm, callback=arm_start_callback)
+    pool.apply_async(ur5_control.powerup_arm, (arm,), callback=arm_start_callback, error_callback=handle_error)
     global ledsys
     ledsys = LEDSystem()
-    pool.apply_async(ledsys.init, callback=led_start_callback)
+    #pool.apply_async(ledsys.init, callback=led_start_callback)
     #pool.apply_async(sensor_control.init, callback=sensor_start_callback)
     jbs = JukeboxSearch()
     
@@ -297,38 +321,85 @@ def setup_server(pool):
     
     if led_ready is False:
         fprint("waiting for " + "LED controller initialization" + " to complete...", sendqueue=to_server_queue)
-        while led_ready is False:
+        ledsys.init()
-            sleep(0.1)
+        led_ready = True
     fprint("LED controllers initialized.", sendqueue=to_server_queue)
-    #to_server_queue.put("[log] LED controllers initialized.")
+
-    sensor_ready = True
     if sensor_ready is False:
         fprint("waiting for " + "Sensor Initialization" + " to complete...", sendqueue=to_server_queue)
-        while sensor_ready is False:
+        global mbconn
-            sleep(0.1)
+        mbconn = ModbusClient(host="localhost", port=502, unit_id=1, auto_open=True, auto_close=True)
     fprint("Sensors initialized.", sendqueue=to_server_queue)
 
     if camera_ready is False:
         fprint("waiting for " + "Camera initilization" + " to complete...", sendqueue=to_server_queue)
-        camera = process_video.qr_reader(config["cameras"]["banner"]["ip"], config["cameras"]["banner"]["port"])
+        camera = process_video.qr_reader(config["cameras"]["banner"]["ip"], int(config["cameras"]["banner"]["port"]))
 
     fprint("Camera initialized.", sendqueue=to_server_queue)
 
-    arm_ready = True
+    #arm_ready = True
     if arm_ready is False:
-        fprint("waiting for " + "UR5 initilization" + " to complete...", sendqueue=to_server_queue)
+        fprint("waiting for " + "UR5 powerup" + " to complete...", sendqueue=to_server_queue)
         while arm_ready is False:
             sleep(0.1)
+
+    ur5_control.init_arm(arm)
     fprint("Arm initialized.", sendqueue=to_server_queue)
 
 
     
     return True
 
+def handle_error(error):
+	print(error, flush=True)
+
+def get_sensors():
+    global mbconn
+    global sensors
+    oldsens = sensors
+    """
+    port 1: 256
+    port 2: 272
+    port 3: 288
+    port 4: 304
+    port 5: 320
+    port 6: 336
+    port 7: 352
+    port 8: 368
+    
+    """
+    out = list()
+    for reg in [352, 288, 304, 368]:
+        val = mbconn.read_holding_registers(reg)
+        if val == 1:
+            out.append(1)
+        else:
+            out.append(0)
+
+    sensors = out
+
+    for x in range(len(oldsens)):
+        if oldsens[x] == 0 and out[x] == 1:
+            # cable newly detected on tray
+            return x
+        
+    return -1
+def get_open_spot(sensordata):
+    for x in range(len(sensordata)):
+        sens = sensordata[x]
+        if not sens:
+            return x
+        
+    # if we get here, every spot is full
+        
+    return False
     
 
 def mainloop_server(pool):
     # NON-blocking loop
+    global ring_animation
+    global led_set_mode
+    global just_placed
     global config
     global counter
     global killme
@@ -342,26 +413,30 @@ def mainloop_server(pool):
     global camera_ready
     global cable_search_ready
     global cable_list
+    global mainloop_get
+    global cable_list_state
 
     if killme.value > 0:
         killall()
 
     if mode == "Startup":
-        counter = 54
+        #counter = 54 # remove for demo
         if counter < 54:
             # scanning cables
-            
+            ring_animation = counter
+            led_set_mode = "GrabA"
             if arm_state is None:
-                #pool.apply_async(arm.get cable to camera, callback=arm_start_callback)
+                #pool.apply_async(arm_start_callback, ("",))
-                #ur5_control.goto_holder_index(arm)
+                arm_ready = False
-                #ur5 get item
+                pool.apply_async(ur5_control.to_camera, (arm,counter), callback=arm_start_callback, error_callback=handle_error)
-                # ur5 bring to camera
                 fprint("Getting cable index " + str(counter) + " and scanning...")
                 arm_state = "GET"
+                #ur5_control.to_camera(arm, counter)
+                #arm_ready = True
 
             elif arm_ready and arm_state == "GET":
                 fprint("Looking for QR code...")
-                pool.apply_async(camera.read_qr, (30,), callback=camera_start_callback)
+                pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
                 arm_ready = False
 
             elif camera_ready:
@@ -374,7 +449,9 @@ def mainloop_server(pool):
                 else:
                     cable_list.append(scan_value)
                 fprint(scan_value)
-                #pool.apply_async(arm.return cable, callback=arm_start_callback)
+                pool.apply_async(ur5_control.return_camera, (arm,counter), callback=arm_start_callback, error_callback=handle_error)
+                #ur5_control.return_camera(arm, counter)
+                #arm_ready = True
                 arm_state = "RETURN"
                 camera_ready = False
 
@@ -386,6 +463,8 @@ def mainloop_server(pool):
                 pass
         else:
             # scanned everything
+            ring_animation = None
+            led_set_mode == "idle"
             tmp = [
                     # Actual cables in Jukebox
                     "BLTF-1LF-006-RS5",
@@ -418,8 +497,13 @@ def mainloop_server(pool):
                 ]
             while len(tmp) < 54:
                 tmp.append(False) # must have 54 entries
-            cable_list = tmp # remove for real demo
+
-            pool.apply_async(get_specs.get_multi, (tmp, 0.3), callback=cable_search_callback)
+            #cable_list = tmp # comment out for real demo
+                
+            for idx in range(len(cable_list)):
+                cable_list_state.append(True)
+
+            pool.apply_async(get_specs.get_multi, (cable_list, 0.3), callback=cable_search_callback, error_callback=handle_error)
             mode = "Parsing"
             fprint("All cables scanned. Finding & parsing datasheets...")
     if mode == "Parsing":
@@ -458,7 +542,7 @@ def mainloop_server(pool):
                     
                     fprint("All cables added to database.")
                     mode = "Idle"
-                    serverproc = Process(target=start_server_socket, args=(cable_list,))
+                    serverproc = Process(target=start_server_socket, args=(cable_list,), error_callback=handle_error)
                     serverproc.start()
                 else:
                     # TODO: manual input
@@ -468,13 +552,104 @@ def mainloop_server(pool):
     if mode == "Idle":
         # do nothing
         if arm_ready is False:
-            pool.apply_async(ur5_control.move_to_home, (arm,), callback=arm_start_callback)
+            pool.apply_async(ur5_control.move_to_home, (arm,), callback=arm_start_callback, error_callback=handle_error)
-            arm_ready = True
+            #arm_ready = True
 
+        else:
+            global mainloop_get
+            newtube = get_sensors()
+            if newtube >= 0 and newtube != just_placed:
+                # need to return a cable
+                mainloop_get.put(("return", newtube))
+                just_placed = -1
+            
+            if not mainloop_get.empty():
+                action, get_cable = mainloop_get.get()
+                if get_cable > -1:
+                    global sensors
+                    if action == "pickup":
+                        spot = get_open_spot(sensors)
+                        if spot is not False:
+                            arm_ready = False
+                            pool.apply_async(ur5_control.pick_up_routine, (arm, get_cable, True, spot), callback=arm_start_callback, error_callback=handle_error)
+                            mode = "Pickup"
+                            cable_list_state[get_cable] = False # mark as removed
+                            get_sensors(sensors,flag=get_open_spot(sensors))
+
+                    if action == "return":
+                        arm_ready = False
+                        pool.apply_async(ur5_control.return_routine, (arm, get_cable), callback=arm_start_callback, error_callback=handle_error)
+                        mode = "ReturnC"
             else:
                 # LED idle anim
                 pass
 
+    if mode == "Pickup":
+        # complete
+        if arm_ready == True:
+            mode = "Idle"
+            arm_ready = False
+            
+        else:
+            # getting cable and bringing to tray
+            # led animation
+            pass
+    
+    if mode == "ReturnC":
+        # complete
+        if arm_ready == True:
+            mode = "Scan"
+            arm_ready = False
+            camera_ready = False
+            pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
+            
+        else:
+            # getting cable from and bringing to camera
+            # led animation
+            pass
+
+    if mode == "Scan":
+        if camera_ready == True:
+            global scan_value
+            if scan_value is False:
+                # unable to scan ???? not good
+                fprint("Unable to scan cable. Gonna retry.")
+                camera_ready = False
+                pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
+                pass
+            elif scan_value.find("bldn.app/") > -1:
+                scan_value = scan_value[scan_value.find("bldn.app/")+9:]
+            
+            fprint("Got cable: " + scan_value)
+
+            for idx in range(len(cable_list)):
+                cable = cable_list[idx]
+                if cable == scan_value and cable_list_state[idx] == False:
+                    cable_list_state[idx] = True # mark cable as returned
+                    arm_ready = False
+                    pool.apply_async(ur5_control.return_camera, (arm, idx), callback=arm_start_callback, error_callback=handle_error)
+                    mode = "Return"
+                    break
+
+            
+
+    if mode == "Return":
+        if arm_ready == True:
+            mode = "Idle"
+            arm_ready = False
+            # movement finished
+
+        else: 
+            # cable going from camera to holder
+            # led animation
+            pass
+            
+    if True:
+        # do every loop!
+        if ring_animation is not None and ledsys.mode != "idle":
+            ledsys.mainloop(None, ring_animation)
+        elif ring_animation is not None:
+            ledsys.mainloop(led_set_mode, ring_animation)
         
 
 
@@ -584,14 +759,15 @@ class LogExceptions(object):
         return result
 
 class LoggingPool(Pool):
-    def apply_async(self, func, args=(), kwds={}, callback=None):
+    def apply_async(self, func, args=(), kwds={}, callback=None, error_callback=None):
-        return Pool.apply_async(self, LogExceptions(func), args, kwds, callback)
+        return Pool.apply_async(self, LogExceptions(func), args, kwds, callback, error_callback)
 
 
 if __name__ == "__main__":
     #sys.stdout = Logger(filename="output.log")
     #sys.stderr = Logger(filename="output.log")
     #log_to_stderr(logging.DEBUG)
+    
     fprint("Starting Jukebox control system...")
     with open('config.yml', 'r') as fileread:
         #global config
@@ -622,7 +798,13 @@ if __name__ == "__main__":
             fprint("Starting in server mode.")
             if setup_server(pool):
                 fprint("Entering main loop...")
+                start = 0
+                speed = config["loopspeed"]
                 while(keeprunning):
+                    start = uptime()
                     mainloop_server(pool)
+                    # limit to certain "framerate"
+                    while start + 1.0/speed < uptime():
+                        pass