Led strip (#58)

* Import lovely WS2812  library for Jetson from random github man

* Implemented WS2812 and it builds

* Hopefully fixed runtime errors, needs actual testing

* Added off function and brightness control

* Add customizable spi bus and device to WS2812 library

* Add HSV rainbow cycle, implemented hermanoid's changes

* Rainbow cycle precalculations, limit the brightness

* Rainbow and other animations loop, smoother rainbow, boot up animation

* New library for new RGB strip

* Used new library in led_controller node

* Testing needed, Fixed RGB to RGBW scaling, threading, pulse anim, solid anim, off anim

* Fixed timing

* Fixed color of new LEDs

* Added Ring and Strip support

* Corrected number of LEDs in ring

---------

Co-authored-by: Lucas Niewohner <lucasniewohner@gmail.com>
Co-authored-by: Zac Stanton <zac71113@gmail.com>

Author: 3 people

src/rov/rov_led_controller/rov_led_controller/SK6812RGBW.py

@@ -0,0 +1,100 @@
+import spidev
+import sys
+import time
+
+class SPItoSK():
+    def __init__(self, numLeds, bus = 1, device = 0):
+        self.ledCount = numLeds
+        self.ledBrightness = 1.0
+        self.spi = spidev.SpiDev()
+        self.spi.open(bus, device)
+        self.spi.max_speed_hz = 2500000
+        # string of bits to send to spi
+        self.binMsg = 0b100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100
+        for i in range(numLeds - 1):
+            # 96 bits per led, 3 bits per real bit
+            self.binMsg = (self.binMsg << 96) | 0b100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100
+
+    def __del__(self):
+        self.spi.close()
+        print("destructor")
+    
+    def set_LED_color(self, ledNum, R, G, B, W):
+        """
+        Adjust self.binMsg to the color desired
+        :param ledNum: the led to change starting with index 0
+        :param R, G, B, W: RGBW values from 0-255
+        """
+        if (R > 255) or (G > 255) or (B > 255) or (W > 255) or (R < 0) or (G < 0) or (B < 0) or (W < 0):
+            print("RGBW values must be in the range 0-255")
+            sys.exit(-1)
+        msgStartPos = (ledNum * 96) + 2
+        # Set G in binary message
+        self._formatBinMsg(msgStartPos, int(round(G * self.ledBrightness)))
+        # Set R in binary message
+        self._formatBinMsg(msgStartPos + 24, int(round(R * self.ledBrightness)))
+        # Set B in binary message
+        self._formatBinMsg(msgStartPos + (24 * 2), int(round(B * self.ledBrightness)))
+        # Set W in binary message
+        self._formatBinMsg(msgStartPos + (24 * 3), int(round(W * self.ledBrightness)))
+
+        # debug
+        # print("led_num: %d, R: %d G: %d B: %d W: %d" % (ledNum, round(R * self.ledBrightness), round(G * self.ledBrightness), round(B * self.ledBrightness), round(W * self.ledBrightness)))
+
+    def LED_show(self):
+        """
+        Signals the LEDs
+        """
+        bytesToSend = []
+        binMsgCpy = self.binMsg
+        while binMsgCpy:
+            bytesToSend.insert(0, binMsgCpy & ((1 << 8) - 1))  # Extract lowest 8 bits
+            binMsgCpy >>= 8  # Shift right by 8 bits
+
+        self.spi.xfer3(bytesToSend, 2500000, 0, 8)
+
+        # debug
+        # print(bin(self.binMsg))
+        # offString = 100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100
+        # print()
+        # for i in bytesToSend:  
+        #     print(bin(i))
+        # time.sleep(80e-6)
+
+    def set_brightness(self, brightness):
+        """
+        Set the global brightness of LEDs
+        :param brightness: Brightness of LEDs 0-1, clamps if values are outside the range
+        """
+        brightness = max(0.0, min(brightness, 1.0))
+        self.ledBrightness = brightness
+
+    def LED_OFF_ALL(self):
+        """
+        Turns off LEDs
+        """
+        self.binMsg = 0b100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100
+        for i in range(self.ledCount - 1):
+            # 96 bits per led, 3 bits per real bit
+            self.binMsg = (self.binMsg << 96) | 0b100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100100
+        self.LED_show()
+                
+    def _formatBinMsg(self, startPos, colorNum):
+        number = format(colorNum, '08b')
+        for i in range(8):
+            msgIndexPos = startPos + (3 * i)
+            colorBit = number[i]
+            if (colorBit == "0") and bin(self.binMsg)[msgIndexPos:msgIndexPos + 3] == "110":
+                self._flipBit(msgIndexPos)
+            elif (colorBit == "1") and bin(self.binMsg)[msgIndexPos:msgIndexPos + 3] == "100":
+                self._flipBit(msgIndexPos)
+
+    def _flipBit(self, position):
+        mask = 1 << len(bin(self.binMsg)[2:]) - position
+        self.binMsg = self.binMsg ^ mask
+
+    
+if __name__ == "__main__":
+    ledStrip = SPItoSK(1)
+    ledStrip.set_LED_color(0, 255, 255, 255, 255)
+    ledStrip.LED_show()

src/rov/rov_led_controller/rov_led_controller/WS2812.py

@@ -0,0 +1,74 @@
+# lifted from this lovely repo: https://github.com/seitomatsubara/Jetson-nano-WS2812-LED-
+
+import spidev
+import sys
+ 
+class SPItoWS():
+    def __init__(self, ledc, bus=1, device=0):
+        self.led_count = ledc
+        self.X = '' # X is signal of WS281x
+        for i in range(self.led_count):
+            self.X = self.X + "100100100100100100100100100100100100100100100100100100100100100100100100"
+        self.led_brightness = 1.0
+        self.spi = spidev.SpiDev()
+        self.spi.open(bus, device)
+        self.spi.max_speed_hz = 2400000
+
+    def __del__(self):
+        self.spi.close()
+        print("destructor")
+        
+    def _Bytesto3Bytes(self, num, RGB): # num is number of signal, RGB is 8 bits (1 byte) str
+        for i in range(8):
+            if RGB[i] == '0':
+                self.X = self.X[:num * 3 * 8 + i * 3] + '100' + self.X[num * 3 * 8 + i * 3 + 3:]
+            elif RGB[i] == '1':
+                self.X = self.X[:num * 3 * 8 + i * 3] + '110' + self.X[num * 3 * 8 + i * 3 + 3:]
+    
+    def _BytesToHex(self, Bytes):
+        return ''.join(["0x%02X " % x for x in Bytes]).strip()
+    
+    def LED_show(self):
+            Y = []
+            for i in range(self.led_count * 9):
+                Y.append(int(self.X[i*8:(i+1)*8],2))
+            WS = self._BytesToHex(Y)
+            self.spi.xfer3(Y, 2400000,0,8)
+
+    def set_LED_color(self, led_num, R, G, B):
+        if (R > 255 or G > 255 or B > 255):
+            print("Invalid Value: RGB is over 255\n")
+            sys.exit(1)
+        if (led_num > self.led_count - 1):
+            print("Invalid Value: The number is over the number of LED")
+            sys.exit(1)
+        RR = format(round(R * self.led_brightness), '08b')
+        GG = format(round(G * self.led_brightness), '08b')
+        BB = format(round(B * self.led_brightness), '08b')
+        self._Bytesto3Bytes(led_num * 3, GG)
+        self._Bytesto3Bytes(led_num * 3 + 1, RR)
+        self._Bytesto3Bytes(led_num * 3 + 2, BB)
+
+        # debug
+        # print("led_num: %d, R: %d G: %d B: %d" % (led_num, round(R * self.led_brightness), round(G * self.led_brightness), round(B * self.led_brightness)))
+
+    def LED_OFF_ALL(self):
+        self.X = ''
+        for i in range(self.led_count):
+            self.X = self.X + "100100100100100100100100100100100100100100100100100100100100100100100100"
+        self.LED_show()
+
+    def set_brightness(self, brightness):
+        self.led_brightness = brightness
+
+
+if __name__ == "__main__":
+    import time
+    LED_COUNT = 3
+    sig = SPItoWS(LED_COUNT)
+    sig.RGBto3Bytes(0, 255, 0, 0)
+    sig.RGBto3Bytes(1, 0, 255, 0)
+    sig.RGBto3Bytes(2, 0, 0, 255)
+    sig.LED_show()
+    time.sleep(1)
+    sig.LED_OFF_ALL()