Add basic LED animations

config.yml

@@ -159,7 +159,7 @@ led:
       size: 24
       diameter: 63.5
       angle: 0
-      pos: [-65.936, 114.3]
+      pos: [-65.991, 114.3]
     - type: circle
       start: 336
       size: 24
@@ -420,6 +420,116 @@ led:
       size: 70
       length: 600
       angle: 270 # down
-      pos: [300, 300]
+      pos: [375, 300]
-
 
+global_position_offset: [0,0] # default coordinate spce below as center of arm at 0,0 - adjust if necessary
+animation_time: 40
+position_map:
+  - index: 0
+    pos: [-152.4, 263.965]
+  - index: 1
+    pos: [-76.2, 263.965]
+  - index: 2
+    pos: [0, 263.965]
+  - index: 3
+    pos: [76.2, 263.965]
+  - index: 4
+    pos: [152.4, 263.965]
+  - index: 5
+    pos: [-190.5, 197.973]
+  - index: 6
+    pos: [-114.3, 197.973]
+  - index: 7
+    pos: [-38.1, 197.973]
+  - index: 8
+    pos: [38.1, 197.973]
+  - index: 9
+    pos: [114.3, 197.973]
+  - index: 10
+    pos: [190.5, 197.973]
+  - index: 11
+    pos: [-228.6, 131.982]
+  - index: 12
+    pos: [-152.4, 131.982]
+  - index: 13
+    pos: [-76.2, 131.982]
+  - index: 14
+    pos: [0, 131.982]
+  - index: 15
+    pos: [76.2, 131.982]
+  - index: 16
+    pos: [152.4, 131.982]
+  - index: 17
+    pos: [228.6, 131.982]
+  - index: 18
+    pos: [-266.7, 65.991]
+  - index: 19
+    pos: [-190.5, 65.991]
+  - index: 20
+    pos: [-114.3, 65.991]
+  - index: 21
+    pos: [114.3, 65.991]
+  - index: 22
+    pos: [190.5, 65.991]
+  - index: 23
+    pos: [266.7, 65.991]
+  - index: 24
+    pos: [-304.8, 0]
+  - index: 25
+    pos: [-228.6, 0]
+  - index: 26
+    pos: [-152.4, 0]
+  - index: 27
+    pos: [152.4, 0]
+  - index: 28
+    pos: [228.6, 0]
+  - index: 29
+    pos: [304.8, 0]
+  - index: 30
+    pos: [-266.7, -65.991]
+  - index: 31
+    pos: [-190.5, -65.991]
+  - index: 32
+    pos: [-114.3, -65.991]
+  - index: 33
+    pos: [114.3, -65.991]
+  - index: 34
+    pos: [190.5, -65.991]
+  - index: 35
+    pos: [266.7, -65.991]
+  - index: 36
+    pos: [-228.6, -131.982]
+  - index: 37
+    pos: [-152.4, -131.982]
+  - index: 38
+    pos: [-76.2, -131.982]
+  - index: 39
+    pos: [0, -131.982]
+  - index: 40
+    pos: [76.2, -131.982]
+  - index: 41
+    pos: [152.4, -131.982]
+  - index: 42
+    pos: [228.6, -131.982]
+  - index: 43
+    pos: [-190.5, -197.973]
+  - index: 44
+    pos: [-114.3, -197.973]
+  - index: 45
+    pos: [-38.1, -197.973]
+  - index: 46
+    pos: [38.1, -197.973]
+  - index: 47
+    pos: [114.3, -197.973]
+  - index: 48
+    pos: [190.5, -197.973]
+  - index: 49
+    pos: [-152.4, -263.965]
+  - index: 50
+    pos: [-76.2, -263.965]
+  - index: 51
+    pos: [0, -263.965]
+  - index: 52
+    pos: [76.2, -263.965]
+  - index: 53
+    pos: [152.4, -263.965]

led_control.py

@@ -22,6 +22,13 @@ leds_size = None
 leds_normalized = None
 controllers = None
 data = None
+exactdata = None
+rings = None
+ringstatus = None
+mode = "Startup"
+firstrun = True
+changecount = 0
+animation_time = 0
 start = uptime()
 
 def ping(host):
@@ -48,30 +55,50 @@ def map():
     global leds_size
     global leds_normalized
     global controllers
+    global rings
+    global ringstatus
+    global animation_time
+    
 
     with open('config.yml', 'r') as fileread:
         #global config
         config = yaml.safe_load(fileread)
 
+    animation_time = config["animation_time"]
     leds = list()
     leds_size = list()
     controllers = list()
+    rings = list(range(len(config["position_map"])))
+    ringstatus = list(range(len(config["position_map"])))
+    #print(rings)
     #fprint(config["led"]["map"])
+    generate_map = False
+    map = list()
     for shape in config["led"]["map"]:
         if shape["type"] == "circle":
+            
+            if generate_map:
+                map.append((shape["pos"][1],shape["pos"][0]))
             #fprint(shape["pos"])
             anglediv = 360.0 / shape["size"]
             angle = 0
             radius = shape["diameter"] / 2
             lednum = shape["start"]
+            for item in config['position_map']:
+            # Check if the current item's position matches the target position
+                #print(item['pos'],(shape["pos"][1],shape["pos"][0]))
+                if tuple(item['pos']) == (shape["pos"][1],shape["pos"][0]):
+                    rings[item["index"]] = (shape["pos"][1],shape["pos"][0],lednum,lednum+shape["size"]) # rings[index] = x, y, startpos, endpos
+                    ringstatus[item["index"]] = [None, None]
+                    break
             if len(leds) < lednum + shape["size"]:
                 for x in range(lednum + shape["size"] - len(leds)):
                     leds.append(None)
                     leds_size.append(None)
             while angle < 359.999:
                 tmpangle = angle + shape["angle"]
-                x = math.cos(tmpangle * (math.pi / 180.0)) * radius + shape["pos"][0]
+                x = math.cos(tmpangle * (math.pi / 180.0)) * radius + shape["pos"][1] # flip by 90 degress when we changed layout
-                y = math.sin(tmpangle * (math.pi / 180.0)) * radius + shape["pos"][1]
+                y = math.sin(tmpangle * (math.pi / 180.0)) * radius + shape["pos"][0]
                 leds[lednum] = (x,y)
                 lednum = lednum + 1
                 angle = angle + anglediv
@@ -97,7 +124,23 @@ def map():
                 dist += distdiv
                 lednum = lednum + 1
 
+    if generate_map:
+        map = sorted(map, key=lambda x: (-x[1], x[0]))
+        print(map)
+        import matplotlib.pyplot as plt
+        plt.axis('equal')
+        x, y = zip(*map)
+        plt.scatter(x, y, s=12)
+        #plt.plot(x, y, marker='o')
+        #plt.scatter(*zip(*leds), s=3)
+        for i, (x_pos, y_pos) in enumerate(map):
+            plt.text(x_pos, y_pos, str(i), color="red", fontsize=12)
+        plt.savefig("map2.png", dpi=600, bbox_inches="tight")
+        data = {"map": [{"index": i, "pos": str(list(pos))} for i, pos in enumerate(map)]}
+        yaml_str = yaml.dump(data, default_flow_style=False)
+        print(yaml_str)
 
+    print(rings)
     flag = 0
     for x in leds:
         if x is None:
@@ -148,9 +191,10 @@ def init():
     global leds_size
     global controllers
     global data
+    global exactdata
     sender = sacn.sACNsender(fps=config["led"]["fps"], universeDiscovery=False)
     sender.start()  # start the sending thread
-    for x in range(len(controllers)):
+    """for x in range(len(controllers)):
         print("Waiting for the controller at", controllers[x][2], "to be online...", end="")
         count = 0
         while not ping(controllers[x][2]):
@@ -159,7 +203,7 @@ def init():
                 fprint(" ERROR: controller still offline after " + str(count) + " seconds, continuing...")
                 break
         if count < config["led"]["timeout"]:
-            fprint(" done")
+            fprint(" done")"""
     for x in range(len(controllers)):
         print("Activating controller", x, "at", controllers[x][2], "with", controllers[x][1]-controllers[x][0], "LEDs.")
         sender.activate_output(x+1)  # start sending out data
@@ -168,22 +212,26 @@ def init():
 
     # initialize global pixel data list
     data = list()
+    exactdata = list()
     for x in range(len(leds)):
         if leds_size[x] == 3:
+            exactdata.append(None)
             data.append((20,20,127))
         elif leds_size[x] == 4:
+            exactdata.append(None)
             data.append((50,50,255,0))
         else:
+            exactdata.append(None)
             data.append((0,0,0))
     sendall(data)
     #time.sleep(50000)    
     fprint("Running start-up test sequence...")
-    for y in range(100):
+    for y in range(1):
         for x in range(len(leds)):
-            setpixel(0,0,150,x)
+            setpixel(0,60,144,x)
         sendall(data)
         #time.sleep(2)
-        #alloffsmooth()
+    alloffsmooth()
 
 def sendall(datain):
     # send all LED data to all controllers
@@ -260,6 +308,209 @@ def setpixelnow(r, g, b, num):
     setpixel(r,g,b,num)
     senduniverse(data, num)
 
+def setmode(stmode, r=0,g=0,b=0):
+    global mode
+    global firstrun
+    if stmode is not None:
+        if mode != stmode:
+            firstrun = True
+    
+        mode = stmode
+    
+
+def setring(r,g,b,idx):
+    
+    ring = rings[idx]
+    for pixel in range(ring[2],ring[3]):
+        setpixel(r,g,b,pixel)
+    #global data
+    #senduniverse(data, ring[2])
+
+def runmodes(ring = -1, speed = 1):
+    global mode
+    global firstrun
+    global changecount
+    fprint("Mode: " + str(mode))
+    if mode == "Startup":
+        # loading animation. cable check
+        if firstrun:
+            changecount = animation_time * 3
+            firstrun = False
+            for x in range(len(ringstatus)):
+                ringstatus[x] = [True, animation_time]
+            
+        if changecount > 0:
+            fprint(changecount)
+            changecount = fadeorder(0,len(leds), changecount, 0,50,100)
+        else:
+            setmode("Startup2")
+            
+                
+    elif mode == "Startup2":
+        if firstrun:
+            firstrun = False
+            
+        else:
+            for x in range(len(ringstatus)):
+                if ringstatus[x][0]:
+                    setring(0, 50, 100, x)
+                else:
+                    ringstatus[x][1] = fadeall(rings[x][2],rings[x][3], ringstatus[x][1], 100,0,0) # not ready
+
+    elif mode == "StartupCheck":
+        if firstrun:
+            firstrun = False
+            for x in range(len(ringstatus)):
+                ringstatus[x] = [False, animation_time]
+        else:
+            for x in range(len(ringstatus)):
+                if ringstatus[x][0]:
+                    ringstatus[x][1] = fadeall(rings[x][2],rings[x][3], ringstatus[x][1], 0,50,100) # ready  
+                else:
+                    setring(100, 0, 0, x)
+
+    elif mode == "GrabA":
+        if firstrun:
+            firstrun = False
+            changecount = animation_time # 100hz
+        if changecount > 0:
+            changecount = fadeall(rings[ring][2],rings[ring][3], changecount, 100,0,0)
+        else:
+            setring(100,0,0,ring)
+            setmode("GrabB")
+    elif mode == "GrabB":
+        if firstrun:
+            firstrun = False
+            changecount = animation_time # 100hz
+        if changecount > 0:
+            changecount = fadeorder(rings[ring][2],rings[ring][3], changecount, 0,100,0)
+        else:
+            setring(0,100,0,ring)
+            setmode("idle")
+    elif mode == "GrabC":
+        if firstrun:
+            firstrun = False
+            changecount = animation_time # 100hz
+        if changecount > 0:
+            changecount = fadeall(rings[ring][2],rings[ring][3], changecount, 0,50,100)
+        else:
+            setring(0,50,100,ring)
+            setmode("idle")
+    elif mode == "idle":
+        time.sleep(0)
+
+    sendall(data)
+            
+def fadeall(idxa,idxb,sizerem,r,g,b):
+    if sizerem < 1:
+        return 0
+    global exactdata
+    sum = 0
+    for x in range(idxa,idxb):
+        if exactdata[x] is None:
+            exactdata[x] = data[x]
+        old = exactdata[x]
+        dr = (r - old[0])/sizerem
+        sum += abs(dr)
+        dr += old[0]
+        dg = (g - old[1])/sizerem
+        sum += abs(dg)
+        dg += old[1]
+        db = (b - old[2])/sizerem 
+        db += old[2]
+        sum += abs(db)
+        exactdata[x] = (dr, dg, db)
+        #print(new)
+        setpixel(dr, dg, db, x)
+        if sizerem == 1:
+            exactdata[x] = None
+    if sum == 0 and sizerem > 2:
+        sizerem = 2
+    return sizerem - 1
+        
+def fadeorder(idxa,idxb,sizerem,r,g,b):
+    if sizerem < 1:
+        return 0
+    global exactdata
+    drs = 0
+    dgs = 0
+    dbs = 0
+    sum = 0
+    for x in range(idxa,idxb):
+        if exactdata[x] is None:
+            exactdata[x] = data[x]
+        old = exactdata[x]
+        dr = (r - old[0])
+        dg = (g - old[1])
+        db = (b - old[2])
+        drs += dr
+        dgs += dg
+        dbs += db
+    
+    drs /= sizerem
+    dgs /= sizerem
+    dbs /= sizerem
+    sum += abs(drs) + abs(dgs) + abs(dbs)
+    print(drs,dgs,dbs)
+    for x in range(idxa,idxb):
+        old = exactdata[x]
+        new = list(old)
+        if drs > 0:
+            if old[0] + drs > r:
+                new[0] = r
+                drs -= r - old[0]
+            else:
+                new[0] = old[0] + drs
+                drs = 0
+        if dgs > 0:
+            if old[1] + dgs > g:
+                new[1] = g
+                dgs -= g - old[1]
+            else:
+                new[1] = old[1] + dgs
+                dgs = 0
+        if dbs > 0:
+            if old[2] + dbs > b:
+                new[2] = b
+                dbs -= b - old[2]
+            else:
+                new[2] = old[2] + dbs
+                dbs = 0
+
+        if drs < 0:
+            if old[0] + drs < r:
+                new[0] = r
+                drs -= r - old[0]
+            else:
+                new[0] = old[0] + drs
+                drs = 0
+        if dgs < 0:
+            if old[1] + dgs < g:
+                new[1] = g
+                dgs -= g - old[1]
+            else:
+                new[1] = old[1] + dgs
+                dgs = 0
+        if dbs < 0:
+            if old[2] + dbs < b:
+                new[2] = b
+                dbs -= b - old[2]
+            else:
+                new[2] = old[2] + dbs
+                dbs = 0
+
+        if drs != 0 or dgs != 0 or dbs != 0:
+            exactdata[x] = new
+            setpixel(new[0],new[1],new[2],x)
+        
+        if sizerem == 1:
+            exactdata[x] = None
+
+    if sum == 0 and sizerem > 2:
+        sizerem = 2
+    return sizerem - 1
+
+
 def setpixel(r, g, b, num):
     global data
     global leds_size
@@ -328,16 +579,105 @@ def mapimage(image, fps=90):
     global data
     fastsendall(data)
 
+def mainloop(stmode, ring = -1, fps = 100, preview = False):
+    global start
+    while uptime() - start < 1/fps:
+        time.sleep(0.00001)
+    fprint(1 / (uptime() - start))
+    start = uptime()
+    if mode is not None:
+        setmode(stmode)
+    runmodes(ring)
+    if preview:
+        drawdata()
+
+def drawdata():
+    #tmp = list()
+    #for x in len(leds):
+    #    led = leds[x]
+    #    tmp.append((led[0], led[1], data[x]))
+
+    x = [led[0] for led in leds]
+    y = [led[1] for led in leds]
+    colors = data
+    colors_normalized = [(x[0]/255, x[1]/255, x[2]/255) for x in colors]
+    # Plot the points
+    plt.scatter(x, y, c=colors_normalized)
+
+    # Optional: add grid, title, and labels
+    plt.grid(True)
+    plt.title('Colored Points')
+    plt.xlabel('X')
+    plt.ylabel('Y')
+    plt.show()
+    plt.savefig("map3.png", dpi=50, bbox_inches="tight")
+    plt.clf()
+
+def startup_animation(show):
+
+    
+    stmode = "Startup"
+    mainloop(stmode, preview=show)
+    while mode == "Startup":
+        mainloop(None, preview=show)
+    for x in range(54):
+        ringstatus[x][0] = False
+        mainloop(None, preview=show)
+
+    for x in range(animation_time):
+        mainloop(None, preview=show)
+    clear_animations()
+    stmode = "StartupCheck"
+    mainloop(stmode, preview=show)
+    clear_animations()
+
+def clear_animations():
+    for x in range(len(leds)):
+        exactdata[x] = None
+
+def do_animation(stmode, ring=-1):
+    mainloop(stmode, ring, preview=show)
+    wait_for_animation(ring)
+
+def start_animation(stmode, ring=-1):
+    mainloop(stmode, ring, preview=show)
+
+def wait_for_animation(ring=-1):
+    while mode != "idle":
+        mainloop(None, ring, preview=show)
 
 if __name__ == "__main__":
     init()
-    cap = cv2.VideoCapture('output.mp4')
+    import matplotlib.pyplot as plt
+    """cap = cv2.VideoCapture('badapple.mp4')
     while cap.isOpened():
         ret, frame = cap.read()
         if not ret:
             break
-        mapimage(frame)
+        mapimage(frame, fps=30)"""
+    show = True
+    ring = 1
+    startup_animation(show)
+    for x in range(54):
+        ringstatus[x][0] = True
+        mainloop(None, preview=show)
+    for x in range(animation_time):
+        mainloop(None, preview=show)
+    
+    do_animation("GrabA", 1)
+
+    do_animation("GrabA", 5)
+    start_animation("GrabC", 1)
+    
+    wait_for_animation(1)
+    do_animation("GrabC", 5)
     
-    time.sleep(1)
     close()
     #sys.exit(0)
+
+
+    # blue : default
+    # green : target
+    # yellow : crosshair
+    # red : missing 
+    # uninitialized : red/purple?