Merge branch 'main' of https://git.myitr.org/Jukebox/jukebox-software

led_control.py

@@ -14,24 +14,32 @@ import cv2
 import numpy as np
 from uptime import uptime
 
-sender = None
-debug = True
-config = None
-leds = None
-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):
+
+class LEDSystem():
+    sender = None
+    debug = True
+    config = None
+    leds = None
+    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 __init__(self):
+        self.start = uptime()
+        #self.init()
+        #return self
+
+    def ping(self, host):
         #Returns True if host (str) responds to a ping request.
 
         # Option for the number of packets as a function of
@@ -49,32 +57,23 @@ def ping(host):
 
         return subprocess.call(command, stdout=subprocess.DEVNULL, stderr=subprocess.STDOUT) == 0
 
-def map():
-    global config
-    global leds
-    global leds_size
-    global leds_normalized
-    global controllers
-    global rings
-    global ringstatus
-    global animation_time
-    
+    def map(self):
 
         with open('config.yml', 'r') as fileread:
             #global config
-        config = yaml.safe_load(fileread)
+            self.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"])))
+        self.animation_time = self.config["animation_time"]
+        self.leds = list()
+        self.leds_size = list()
+        self.controllers = list()
+        self.rings = list(range(len(self.config["position_map"])))
+        self.ringstatus = list(range(len(self.config["position_map"])))
         #print(rings)
         #fprint(config["led"]["map"])
         generate_map = False
         map = list()
-    for shape in config["led"]["map"]:
+        for shape in self.config["led"]["map"]:
             if shape["type"] == "circle":
 
                 if generate_map:
@@ -84,22 +83,22 @@ def map():
                 angle = 0
                 radius = shape["diameter"] / 2
                 lednum = shape["start"]
-            for item in config['position_map']:
+                for item in self.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]
+                        self.rings[item["index"]] = (shape["pos"][1],shape["pos"][0],lednum,lednum+shape["size"]) # rings[index] = x, y, startpos, endpos
+                        self.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)
+                if len(self.leds) < lednum + shape["size"]:
+                    for x in range(lednum + shape["size"] - len(self.leds)):
+                        self.leds.append(None)
+                        self.leds_size.append(None)
                 while angle < 359.999:
                     tmpangle = angle + shape["angle"]
                     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"][0]
-                leds[lednum] = (x,y)
+                    self.leds[lednum] = (x,y)
                     lednum = lednum + 1
                     angle = angle + anglediv
 
@@ -112,13 +111,13 @@ def map():
                 xmov = math.cos(angle * (math.pi / 180.0)) * distdiv
                 ymov = math.sin(angle * (math.pi / 180.0)) * distdiv
                 pos = shape["pos"]
-            if len(leds) < lednum + shape["size"]:
-                for x in range(lednum + shape["size"] - len(leds)):
-                    leds.append(None)
-                    leds_size.append(None)
+                if len(self.leds) < lednum + shape["size"]:
+                    for x in range(lednum + shape["size"] - len(self.leds)):
+                        self.leds.append(None)
+                        self.leds_size.append(None)
 
                 while dist < length:
-                leds[lednum] = (pos[0], pos[1])
+                    self.leds[lednum] = (pos[0], pos[1])
                     pos[0] += xmov
                     pos[1] += ymov
                     dist += distdiv
@@ -140,276 +139,262 @@ def map():
             yaml_str = yaml.dump(data, default_flow_style=False)
             print(yaml_str)
 
-    print(rings)
+        print(self.rings)
         flag = 0
-    for x in leds:
+        for x in self.leds:
             if x is None:
                 flag = flag + 1
         if flag > 0:
             fprint("Warning: Imperfect LED map ordering. Hiding undefined lights.")
-        for x in range(len(leds)):
-            if leds[x] is None:
-                leds[x] = (0, 0)
+            for x in range(len(self.leds)):
+                if self.leds[x] is None:
+                    self.leds[x] = (0, 0)
 
 
         #leds = tmpleds.reverse()
         #fprint(leds)
 
         # controller mapping
-    for ctrl in config["led"]["controllers"]:
-        if len(controllers) < ctrl["universe"]+1:
-            for x in range(ctrl["universe"]+1 - len(controllers)):
-                controllers.append(None)
+        for ctrl in self.config["led"]["controllers"]:
+            if len(self.controllers) < ctrl["universe"]+1:
+                for x in range(ctrl["universe"]+1 - len(self.controllers)):
+                    self.controllers.append(None)
 
-        controllers[ctrl["universe"]] = (ctrl["ledstart"],ctrl["ledend"]+1,ctrl["ip"])
+            self.controllers[ctrl["universe"]] = (ctrl["ledstart"],ctrl["ledend"]+1,ctrl["ip"])
             for x in range(ctrl["ledstart"],ctrl["ledend"]+1):
-            leds_size[x] = len(ctrl["mode"])
+                self.leds_size[x] = len(ctrl["mode"])
         #fprint(controllers)
 
-    if(debug):
+        if(self.debug):
             import matplotlib.pyplot as plt
             plt.axis('equal')
-        for ctrl in controllers:
-            plt.scatter(*zip(*leds[ctrl[0]:ctrl[1]]), s=2)
+            for ctrl in self.controllers:
+                plt.scatter(*zip(*self.leds[ctrl[0]:ctrl[1]]), s=2)
             #plt.scatter(*zip(*leds), s=3)
             plt.savefig("map.png", dpi=600, bbox_inches="tight")
 
-    leds_adj = [(x-min([led[0] for led in leds]), # push to zero start
-                    y-min([led[1] for led in leds]) )
-                   for x, y in leds]
+        leds_adj = [(x-min([led[0] for led in self.leds]), # push to zero start
+                        y-min([led[1] for led in self.leds]) )
+                       for x, y in self.leds]
 
-    leds_normalized = [(x / max([led[0] for led in leds_adj]), 
+        self.leds_normalized = [(x / max([led[0] for led in leds_adj]), 
                         y / max([led[1] for led in leds_adj]))
                        for x, y in leds_adj]
         #return leds, controllers
 
-def init():
-    map()
-    global sender
-    global config
-    global leds
-    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)):
-        print("Waiting for the controller at", controllers[x][2], "to be online...", end="")
+    def init(self):
+        self.map()
+        self.sender = sacn.sACNsender(fps=self.config["led"]["fps"], universeDiscovery=False)
+        self.sender.start()  # start the sending thread
+        """for x in range(len(self.controllers)):
+            print("Waiting for the controller at", self.controllers[x][2], "to be online...", end="")
             count = 0
-        while not ping(controllers[x][2]):
+            while not ping(self.controllers[x][2]):
                 count = count + 1
-            if count >= config["led"]["timeout"]:
+                if count >= self.config["led"]["timeout"]:
                     fprint(" ERROR: controller still offline after " + str(count) + " seconds, continuing...")
                     break
-        if count < config["led"]["timeout"]:
+            if count < self.config["led"]["timeout"]:
                 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
-        sender[x+1].destination = controllers[x][2]
-    sender.manual_flush = True
+        for x in range(len(self.controllers)):
+            print("Activating controller", x, "at", self.controllers[x][2], "with", self.controllers[x][1]-self.controllers[x][0], "LEDs.")
+            self.sender.activate_output(x+1)  # start sending out data
+            self.sender[x+1].destination = self.controllers[x][2]
+        self.sender.manual_flush = True
 
         # 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))
+        self.data = list()
+        self.exactdata = list()
+        for x in range(len(self.leds)):
+            if self.leds_size[x] == 3:
+                self.exactdata.append(None)
+                self.data.append((20,20,127))
+            elif self.leds_size[x] == 4:
+                self.exactdata.append(None)
+                self.data.append((50,50,255,0))
             else:
-            exactdata.append(None)
-            data.append((0,0,0))
-    sendall(data)
+                self.exactdata.append(None)
+                self.data.append((0,0,0))
+        self.sendall(self.data)
         #time.sleep(50000)    
         fprint("Running start-up test sequence...")
         for y in range(1):
-        for x in range(len(leds)):
-            setpixel(0,60,144,x)
-        sendall(data)
+            for x in range(len(self.leds)):
+                self.setpixel(0,60,144,x)
+            self.sendall(self.data)
             #time.sleep(2)
-    alloffsmooth()
+        self.alloffsmooth()
 
-def sendall(datain):
+    def sendall(self, datain):
         # send all LED data to all controllers
         # data must have all LED data in it as [(R,G,B,)] tuples in an array, 1 tuple per pixel
-    global controllers
-    global sender
-    sender.manual_flush = True
-    for x in range(len(controllers)):
-        sender[x+1].dmx_data = list(sum(datain[controllers[x][0]:controllers[x][1]] , ())) # flatten the subsection of the data array
+        self.sender.manual_flush = True
+        for x in range(len(self.controllers)):
+            self.sender[x+1].dmx_data = list(sum(datain[self.controllers[x][0]:self.controllers[x][1]] , ())) # flatten the subsection of the data array
 
-    sender.flush()
+        self.sender.flush()
         time.sleep(0.002)
         #sender.flush() # 100% reliable with 2 flushes, often fails with 1
         #time.sleep(0.002)
         #sender.flush()
 
-def fastsendall(datain):
+    def fastsendall(self, datain):
         # send all LED data to all controllers
         # data must have all LED data in it as [(R,G,B,)] tuples in an array, 1 tuple per pixel
-    global controllers
-    global sender
-    sender.manual_flush = False
-    print(datain[controllers[0][0]:controllers[0][1]])
-    for x in range(len(controllers)):
-        sender[x+1].dmx_data = list(sum(datain[controllers[x][0]:controllers[x][1]] , ())) # flatten the subsection of the data array
+        self.sender.manual_flush = False
+        print(datain[self.controllers[0][0]:self.controllers[0][1]])
+        for x in range(len(self.controllers)):
+            self.sender[x+1].dmx_data = list(sum(datain[self.controllers[x][0]:self.controllers[x][1]] , ())) # flatten the subsection of the data array
 
-    sender.flush()
+        self.sender.flush()
 
-def senduniverse(datain, lednum):
+    def senduniverse(self, datain, lednum):
         # send all LED data for 1 controller/universe
         # data must have all LED data in it as [(R,G,B,)] tuples in an array, 1 tuple per pixel
-    global controllers
-    global sender
-    for x in range(len(controllers)):
-        if lednum >= controllers[x][0] and lednum < controllers[x][1]:
-            sender[x+1].dmx_data = list(sum(datain[controllers[x][0]:controllers[x][1]] , ())) # flatten the subsection of the data array
+        for x in range(len(self.controllers)):
+            if lednum >= self.controllers[x][0] and lednum < self.controllers[x][1]:
+                self.sender[x+1].dmx_data = list(sum(datain[self.controllers[x][0]:self.controllers[x][1]] , ())) # flatten the subsection of the data array
 
-    sender.flush()
+        self.sender.flush()
         time.sleep(0.004)
         #sender.flush() # 100% reliable with 2 flushes, often fails with 1
         #time.sleep(0.002)
         #sender.flush()
 
-def alloff():
+    def alloff(self):
         tmpdata = list()
-    for x in range(len(leds)):
-        if leds_size[x] == 3:
+        for x in range(len(self.leds)):
+            if self.leds_size[x] == 3:
                 tmpdata.append((0,0,0))
-        elif leds_size[x] == 4:
+            elif self.leds_size[x] == 4:
                 tmpdata.append((0,0,0,0))
             else:
                 tmpdata.append((0,0,0))
-    sendall(tmpdata)
+        self.sendall(tmpdata)
         #sendall(tmpdata)
         #sendall(tmpdata) #definitely make sure it's off
+        return self
 
-def allon():
-    global data
-    sendall(data)
+    def allon(self):
+        self.sendall(self.data)
+        return self
 
-def alloffsmooth():
-    tmpdata = data
+    def alloffsmooth(self):
+        tmpdata = self.data
         for x in range(256):
-        for x in range(len(data)):
-            setpixel(tmpdata[x][0]-1,tmpdata[x][1]-1,tmpdata[x][2]-1, x)
-        sendall(tmpdata)
+            for x in range(len(self.data)):
+                self.setpixel(tmpdata[x][0]-1,tmpdata[x][1]-1,tmpdata[x][2]-1, x)
+            self.sendall(tmpdata)
 
-    alloff()
+        self.alloff()
+        return self
 
-def setpixelnow(r, g, b, num):
+    def setpixelnow(self, r, g, b, num):
         # slight optimization: send only changed universe
         # unfortunately no way to manual flush data packets to only 1 controller with this sACN library
-    global data
-    setpixel(r,g,b,num)
-    senduniverse(data, num)
+        self.setpixel(r,g,b,num)
+        self.senduniverse(self.data, num)
+        return self
 
-def setmode(stmode, r=0,g=0,b=0):
-    global mode
-    global firstrun
+    def setmode(self, stmode, r=0,g=0,b=0):
         if stmode is not None:
-        if mode != stmode:
-            firstrun = True
+            if self.mode != stmode:
+                self.firstrun = True
 
-        mode = stmode
+            self.mode = stmode
+        return self
 
 
-def setring(r,g,b,idx):
+    def setring(self, r,g,b,idx):
 
-    ring = rings[idx]
+        ring = self.rings[idx]
         for pixel in range(ring[2],ring[3]):
-        setpixel(r,g,b,pixel)
+            self.setpixel(r,g,b,pixel)
         #global data
         #senduniverse(data, ring[2])
+        return self
 
-def runmodes(ring = -1, speed = 1):
-    global mode
-    global firstrun
-    global changecount
-    fprint("Mode: " + str(mode))
-    if mode == "Startup":
+    def runmodes(self, ring = -1, speed = 1):
+        fprint("Mode: " + str(self.mode))
+        if self.mode == "Startup":
             # loading animation. cable check
-        if firstrun:
-            changecount = animation_time * 3
+            if self.firstrun:
+                self.changecount = self.animation_time * 3
                 firstrun = False
-            for x in range(len(ringstatus)):
-                ringstatus[x] = [True, animation_time]
+                for x in range(len(self.ringstatus)):
+                    self.ringstatus[x] = [True, self.animation_time]
 
-        if changecount > 0:
-            fprint(changecount)
-            changecount = fadeorder(0,len(leds), changecount, 0,50,100)
+            if self.changecount > 0:
+                fprint(self.changecount)
+                self.changecount = self.fadeorder(0,len(self.leds), self.changecount, 0,50,100)
             else:
-            setmode("Startup2")
+                self.setmode("Startup2")
 
 
-    elif mode == "Startup2":
-        if firstrun:
-            firstrun = False
+        elif self.mode == "Startup2":
+            if self.firstrun:
+                self.firstrun = False
 
             else:
-            for x in range(len(ringstatus)):
-                if ringstatus[x][0]:
-                    setring(0, 50, 100, x)
+                for x in range(len(self.ringstatus)):
+                    if self.ringstatus[x][0]:
+                        self.setring(0, 50, 100, x)
                     else:
-                    ringstatus[x][1] = fadeall(rings[x][2],rings[x][3], ringstatus[x][1], 100,0,0) # not ready
+                        self.ringstatus[x][1] = self.fadeall(self.rings[x][2],self.rings[x][3], self.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]
+        elif self.mode == "StartupCheck":
+            if self.firstrun:
+                self.firstrun = False
+                for x in range(len(self.ringstatus)):
+                    self.ringstatus[x] = [False, self.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  
+                for x in range(len(self.ringstatus)):
+                    if self.ringstatus[x][0]:
+                        self.ringstatus[x][1] = self.fadeall(self.rings[x][2],self.rings[x][3], self.ringstatus[x][1], 0,50,100) # ready  
                     else:
-                    setring(100, 0, 0, x)
+                        self.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)
+        elif self.mode == "GrabA":
+            if self.firstrun:
+                self.firstrun = False
+                self.changecount = self.animation_time # 100hz
+            if self.changecount > 0:
+                self.changecount = self.fadeall(self.rings[ring][2],self.rings[ring][3], self.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)
+                self.setring(100,0,0,ring)
+                self.setmode("GrabB")
+        elif self.mode == "GrabB":
+            if self.firstrun:
+                self.firstrun = False
+                self.changecount = self.animation_time # 100hz
+            if self.changecount > 0:
+                self.changecount = self.fadeorder(self.rings[ring][2],self.rings[ring][3], self.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)
+                self.setring(0,100,0,ring)
+                self.setmode("idle")
+        elif self.mode == "GrabC":
+            if self.firstrun:
+                self.firstrun = False
+                self.changecount = self.animation_time # 100hz
+            if self.changecount > 0:
+                self.changecount = self.fadeall(self.rings[ring][2],self.rings[ring][3], self.changecount, 0,50,100)
             else:
-            setring(0,50,100,ring)
-            setmode("idle")
-    elif mode == "idle":
+                self.setring(0,50,100,ring)
+                self.setmode("idle")
+        elif self.mode == "idle":
             time.sleep(0)
 
-    sendall(data)
+        self.sendall(self.data)
+        return self
 
-def fadeall(idxa,idxb,sizerem,r,g,b):
+    def fadeall(self, 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]
+            if self.exactdata[x] is None:
+                self.exactdata[x] = self.data[x]
+            old = self.exactdata[x]
             dr = (r - old[0])/sizerem
             sum += abs(dr)
             dr += old[0]
@@ -419,27 +404,26 @@ def fadeall(idxa,idxb,sizerem,r,g,b):
             db = (b - old[2])/sizerem 
             db += old[2]
             sum += abs(db)
-        exactdata[x] = (dr, dg, db)
+            self.exactdata[x] = (dr, dg, db)
             #print(new)
-        setpixel(dr, dg, db, x)
+            self.setpixel(dr, dg, db, x)
             if sizerem == 1:
-            exactdata[x] = None
+                self.exactdata[x] = None
         if sum == 0 and sizerem > 2:
             sizerem = 2
         return sizerem - 1
 
-def fadeorder(idxa,idxb,sizerem,r,g,b):
+    def fadeorder(self, 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]
+            if self.exactdata[x] is None:
+                self.exactdata[x] = self.data[x]
+            old = self.exactdata[x]
             dr = (r - old[0])
             dg = (g - old[1])
             db = (b - old[2])
@@ -453,7 +437,7 @@ def fadeorder(idxa,idxb,sizerem,r,g,b):
         sum += abs(drs) + abs(dgs) + abs(dbs)
         print(drs,dgs,dbs)
         for x in range(idxa,idxb):
-        old = exactdata[x]
+            old = self.exactdata[x]
             new = list(old)
             if drs > 0:
                 if old[0] + drs > r:
@@ -500,20 +484,18 @@ def fadeorder(idxa,idxb,sizerem,r,g,b):
                     dbs = 0
 
             if drs != 0 or dgs != 0 or dbs != 0:
-            exactdata[x] = new
-            setpixel(new[0],new[1],new[2],x)
+                self.exactdata[x] = new
+                self.setpixel(new[0],new[1],new[2],x)
 
             if sizerem == 1:
-            exactdata[x] = None
+                self.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
+    def setpixel(self, r, g, b, num):
         # constrain values
         if r < 0:
             r = 0
@@ -528,29 +510,28 @@ def setpixel(r, g, b, num):
         elif b > 255:
             b = 255
 
-    if leds_size[num] == 3:
-        data[num] = (int(r), int(g), int(b))
-    elif leds_size[num] == 4: # cut out matching white and turn on white pixel instead
-        data[num] = (( int(r) - int(min(r,g,b)), int(g) - int(min(r,g,b)), int(b) - int(min(r,g,b)), int(min(r,g,b))) )
+        if self.leds_size[num] == 3:
+            self.data[num] = (int(r), int(g), int(b))
+        elif self.leds_size[num] == 4: # cut out matching white and turn on white pixel instead
+            self.data[num] = (( int(r) - int(min(r,g,b)), int(g) - int(min(r,g,b)), int(b) - int(min(r,g,b)), int(min(r,g,b))) )
         else:
-        data[num] = (int(r), int(g), int(b))
+            self.data[num] = (int(r), int(g), int(b))
+        return self
 
-
-def close():
-    global sender
+    def close(self):
         time.sleep(0.5)
-    sender.stop()
+        self.sender.stop()
+        return self
 
-def mapimage(image, fps=90):
-    global start
-    while uptime() - start < 1/fps:
+    def mapimage(self, image, fps=90):
+        while uptime() - self.start < 1/fps:
             time.sleep(0.00001)
-    fprint(1 / (uptime() - start))
-    start = uptime()
+        fprint(1 / (uptime() - self.start))
+        self.start = uptime()
         minsize = min(image.shape[0:2])
         leds_normalized2 = [(x * minsize, 
                             y * minsize)
-                        for x, y in leds_normalized]
+                            for x, y in self.leds_normalized]
 
         cv2.imshow("video", image)
         cv2.waitKey(1)
@@ -567,39 +548,39 @@ def mapimage(image, fps=90):
                 #avgx += x
                 #avgy += y
                 color = tuple(image[y, x])
-            setpixel(color[2]/2,color[1]/2,color[0]/2,xx) # swap b & r
+                self.setpixel(color[2]/2,color[1]/2,color[0]/2,xx) # swap b & r
                 #print(color)
             else:
                 #avgx += x
                 #avgy += y
-            setpixel(0,0,0,xx)
+                self.setpixel(0,0,0,xx)
         #avgx /= len(leds)
         #avgy /= len(leds)
         #print((avgx,avgy, max([led[0] for led in leds_adj]), max([led[1] for led in leds_adj]) , min(image.shape[0:2]) ))
-    global data
-    fastsendall(data)
+        self.fastsendall(self.data)
+        return self
 
-def mainloop(stmode, ring = -1, fps = 100, preview = False):
-    global start
-    while uptime() - start < 1/fps:
+    def mainloop(self, stmode, ring = -1, fps = 100, preview = False):
+        while uptime() - self.start < 1/fps:
             time.sleep(0.00001)
-    fprint(1 / (uptime() - start))
-    start = uptime()
-    if mode is not None:
-        setmode(stmode)
-    runmodes(ring)
+        fprint(1 / (uptime() - self.start))
+        self.start = uptime()
+        if self.mode is not None:
+            self.setmode(stmode)
+        self.runmodes(ring)
         if preview:
-        drawdata()
+            self.drawdata()
+        return self
 
-def drawdata():
+    def drawdata(self):
         #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
+        x = [led[0] for led in self.leds]
+        y = [led[1] for led in self.leds]
+        colors = self.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)
@@ -612,42 +593,48 @@ def drawdata():
         plt.show()
         plt.savefig("map3.png", dpi=50, bbox_inches="tight")
         plt.clf()
+        return self
 
-def startup_animation(show):
+    def startup_animation(self, show):
 
 
         stmode = "Startup"
-    mainloop(stmode, preview=show)
-    while mode == "Startup":
-        mainloop(None, preview=show)
+        self.mainloop(stmode, preview=show)
+        while self.mode == "Startup":
+            self.mainloop(None, preview=show)
         for x in range(54):
-        ringstatus[x][0] = False
-        mainloop(None, preview=show)
+            self.ringstatus[x][0] = False
+            self.mainloop(None, preview=show)
 
-    for x in range(animation_time):
-        mainloop(None, preview=show)
-    clear_animations()
+        for x in range(self.animation_time):
+            self.mainloop(None, preview=show)
+        self.clear_animations()
         stmode = "StartupCheck"
-    mainloop(stmode, preview=show)
-    clear_animations()
+        self.mainloop(stmode, preview=show)
+        self.clear_animations()
+        return self
 
-def clear_animations():
-    for x in range(len(leds)):
-        exactdata[x] = None
+    def clear_animations(self):
+        for x in range(len(self.leds)):
+            self.exactdata[x] = None
+        return self
 
-def do_animation(stmode, ring=-1):
-    mainloop(stmode, ring, preview=show)
-    wait_for_animation(ring)
+    def do_animation(self, stmode, ring=-1):
+        self.mainloop(stmode, ring, preview=show)
+        self.wait_for_animation(ring)
+        return self
 
-def start_animation(stmode, ring=-1):
-    mainloop(stmode, ring, preview=show)
+    def start_animation(self, stmode, ring=-1):
+        self.mainloop(stmode, ring, preview=show)
+        return self
 
-def wait_for_animation(ring=-1):
-    while mode != "idle":
-        mainloop(None, ring, preview=show)
+    def wait_for_animation(self, ring=-1):
+        while self.mode != "idle":
+            self.mainloop(None, ring, preview=show)
+        return self
 
 if __name__ == "__main__":
-    init()
+    
     import matplotlib.pyplot as plt
     """cap = cv2.VideoCapture('badapple.mp4')
     while cap.isOpened():
@@ -657,22 +644,23 @@ if __name__ == "__main__":
     mapimage(frame, fps=30)"""
     show = False
     ring = 1
-    startup_animation(show)
+    ledsys = LEDSystem()
+    ledsys.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)
+        ledsys.ringstatus[x][0] = True
+        ledsys.mainloop(None, preview=show)
+    for x in range(ledsys.animation_time):
+        ledsys.mainloop(None, preview=show)
     
-    do_animation("GrabA", 1)
+    ledsys.do_animation("GrabA", 1)
 
-    do_animation("GrabA", 5)
-    start_animation("GrabC", 1)
+    ledsys.do_animation("GrabA", 5)
+    ledsys.start_animation("GrabC", 1)
     
-    wait_for_animation(1)
-    do_animation("GrabC", 5)
+    ledsys.wait_for_animation(1)
+    ledsys.do_animation("GrabC", 5)
     
-    close()
+    ledsys.close()
     #sys.exit(0)
 
 

run.py

@@ -17,7 +17,7 @@ import signal
 import socket
 from flask import Flask, render_template, request
 import requests
-import led_control
+from led_control import LEDSystem
 import server
 import asyncio
 import json
@@ -36,7 +36,7 @@ killme = None
 #pool = None
 serverproc = None
 camera = None
-
+ledsys = None
 to_server_queue = Queue()
 from_server_queue = Queue()
 
@@ -47,6 +47,8 @@ def arm_start_callback(res):
 def led_start_callback(res):
     global led_ready
     led_ready = True
+    global ledsys
+    ledsys = res
 
 def camera_start_callback(res):
     global camera_ready
@@ -233,7 +235,9 @@ def setup_server(pool):
     global camera
 
     pool.apply_async(ur5_control.init, (config["arm"]["ip"],), callback=arm_start_callback)
-    pool.apply_async(led_control.init, callback=led_start_callback)
+    global ledsys
+    ledsys = LEDSystem()
+    pool.apply_async(ledsys.init, callback=led_start_callback)
     #pool.apply_async(sensor_control.init, callback=sensor_start_callback)
     serverproc = Process(target=start_server_socket)
     serverproc.start()

ur5_control.py

@@ -6,6 +6,7 @@ import numpy as np
 import time
 import os
 import logging
+import yaml
 from urx.robotiq_two_finger_gripper import Robotiq_Two_Finger_Gripper
 import sys
 from util import fprint
@@ -191,11 +192,6 @@ def move_to_polar(start_pos, end_pos):
 
     return rx_intermediate
 
-def degtorad(angle):
-        return angle/180.0 * math.pi
-def radtodeg(angle):
-        return angle*180.0 / math.pi
-
 def move_to_home():
     global rob
 
@@ -222,8 +218,6 @@ def normalize_degree(theta):
     # Return angle
     return normalized_theta
 
-
-
 def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess = (math.pi/2, math.pi/2, 0), l3offset=0):
     # Get limbs and offsets
     
@@ -276,37 +270,61 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess =
 def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0):
     joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz, l3offset=l3offset)
 
+    # Return current positions if coordinates don't make sense
+    if z<0:
+        return rob.getj()
+
     # Joint offsets
     # Base, Shoulder, Elbow, Wrist
     inverse = [1, -1, 1, 1, 1, 1]
     offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0]
-    if radtodeg(joints[1]) > 137:
+
+    if math.degrees(joints[1]) > 137:
         print("CRASH! Shoulder at", joints[1] * 180/math.pi)
     #else:
         #print("Shoulder at", joints[1] * 180/math.pi)
+
     # Return adjusted joint positions
     return [o+j*i for j, o, i in zip(joints, offsets, inverse)]
 
-# gripper angle: from vertical
-# gripper length: from joint to start of grip
-# to flip, you can use flip=True or make gripper angle negative
+def move_arc(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
+    
+    global rob
+    start_joints = rob.getj()
+    end_joint = get_joints_from_xyz_abs(x, y, z, rx, ry, rz)
+
+    n_points = 50
+    intermediate_joints = []   
+    for i in range(0, 6):
+        intermediate_joints.append(np.linspace(start_joints[i], end_joint[i], n_points))
+
+    joints = [joint_position for joint_position in zip(*intermediate_joints)]
+
+    rob.movejs(joints, acc=2, vel=2, radius=0.1)
+
 def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.20+0.018, flip=False):
+    # gripper angle: from vertical
+    # gripper length: from joint to start of grip
+    # to flip, you can use flip=True or make gripper angle negative
+
+    # Determine tool rotation depending on gripper angle
     if gripperangle < 0:
         rz = - math.pi / 2
     else:
         rz = math.pi / 2
 
     if flip:
-        gripperangle = -degtorad(gripperangle)
+        gripperangle = -math.radians(gripperangle)
         grippery = gripperlength - math.cos(gripperangle) * gripperlength
         grippery += math.sin(gripperangle) * limb3
         gripperx = math.sin(gripperangle) * gripperlength + limb3 * 2
         gripperx -= (1-math.cos(gripperangle)) * limb3
         rz = math.pi / 2
         # flip the whole wrist
-        return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle + degtorad(180), l3offset=-gripperx, ry=math.pi/2, rz=rz)
+        return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=-rz)
+
     else:
-        gripperangle = degtorad(gripperangle)
+        gripperangle = math.radians(gripperangle)
         grippery = gripperlength - math.cos(gripperangle) * gripperlength
         grippery -= math.sin(gripperangle) * limb3
         gripperx = math.sin(gripperangle) * gripperlength
@@ -314,15 +332,59 @@ def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.20+0.018, fli
 
         return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz)
     
-
 def goto_holder_index(idx, z=0.05, gripperangle=35, flip=False):
     joint = config["position_map"][idx]
     print("Going to cable holder index", joint["index"], "at position", joint["pos"])   
-    angles = offset_gripper_angle(joint["pos"][1]/1000, joint["pos"][0]/1000, z, gripperangle=gripperangle, flip=flip)
+    
+    safe_move(joint["pos"][1]/1000, joint["pos"][0]/1000, z)
+    #angles = offset_gripper_angle(joint["pos"][1]/1000, joint["pos"][0]/1000, z, gripperangle=gripperangle, flip=flip)
     #rob.movej(angles, acc=2, vel=2)
-    return angles
+    #return angles
     #angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )
 
+def is_flipped():
+    global rob
+    wrist2 = rob.getj()[4]
+
+    if wrist2>0:
+        return True
+    else:
+        return False
+
+def flip():
+    global rob
+
+    # A list of safe positions to flip
+    safe_positions = [(-0.205, -0.108, 0.3),
+                      (0.205, -0.108, 0.3)]
+
+    # Find the closest safe position
+    curr_pos = rob.getl()[:3]
+    def dist_from_robot(pos):
+        x, y, z = pos
+        rx, ry, rz = curr_pos
+        return math.sqrt((rx-x)**2+(ry-y)**2+(rz-z)**2)
+
+    pos_dist_pairs = zip(safe_positions, [dist_from_robot(pos) for pos in safe_positions])
+    safe_pos = min(pos_dist_pairs, key=lambda x:x[1])[0]
+
+    # Flip at safe position
+    rob.movej(offset_gripper_angle(*safe_pos, flip=is_flipped()), vel=2, acc=2) # Move to safe position
+    rob.movej(offset_gripper_angle(*safe_pos, flip=(not is_flipped())), vel=2, acc=2) # Flip gripper
+
+def safe_move(x, y, z):
+    flip_radius = 0.17 # Min radius on which to flip
+    r = math.sqrt(x**2 + y**2) # Get position radius
+
+    # Flip gripper if needed
+    if (r <= flip_radius and is_flipped()) or (r > flip_radius and not is_flipped()):
+        flip()
+
+    global rob
+    rob.movej(offset_gripper_angle(x, y, z, flip=is_flipped()), vel=2, acc=2)
+
+
+
 if __name__ == "__main__":
     
     #rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2)
@@ -356,41 +418,28 @@ if __name__ == "__main__":
         0.0510]
 
     curr_pos = rob.getl()
-    # up/down, 
-    # tool rotation
-    # tool angle (shouldn't need)
-    # rob.set_pos(p1[0:3], acc=0.5, vel=0.5)
-
-    # set_pos_abs(*home_pose)
 
     
-    
-    # angles = get_joints_from_xyz_abs(-0.2, 0, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(-0.2, -0.2, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.6, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.5, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.4, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.3, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.2, 0)
-    # rob.movej(angles, acc=2, vel=2)
-    # angles = get_joints_from_xyz_abs(0, -0.13, 0)
-    # rob.movej(angles, acc=2, vel=2)
     config = None
     joints = []
-    for i in np.linspace(-0.2, -0.7, 50):
+    for i in np.linspace(-0.2, -0.7, 10):
          joints.append(get_joints_from_xyz_abs(i, 0, 0))
-    #rob.movejs(joints, acc=2, vel=2)
-    import yaml
+    # rob.movejs(joints, acc=2, vel=2, radius=0.1)
+
     with open('config.yml', 'r') as fileread:
         #global config
         config = yaml.safe_load(fileread)
 
+    # move_arc(0, 0.3, 0.1)
+    # move_arc(0, -0.3, 0.3)
+
+
+    # for i in range(20):
+    #     goto_holder_index(i, 0.1)
+
+    flip()
+    flip()
+
     #rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2)
     #joints = []