denko 0.13.0

data/src/lib/DenkoIROutESP.cpp

@@ -0,0 +1,26 @@
+//
+// Denko IR output for the ESP8266 and ESP32.
+// Depends on: https://github.com/crankyoldgit/IRremoteESP8266
+// DENKO_IR_OUT must be defeind in DenkoDefines.h
+//
+#include "Denko.h"
+#if defined(DENKO_IR_OUT) && (defined(ESP8266) || defined(ESP32))
+
+#include <IRremoteESP8266.h>
+#include <IRsend.h>
+
+// CMD = 16
+// Send an infrared signal.
+void Denko::irSend(){
+  // Byte 1+ of auxMsg is already little-endian uint16 pulses.
+  uint16_t *pulseArray = reinterpret_cast<uint16_t *>(auxMsg + 1);
+  
+  // Can work on any pin.
+  IRsend infraredOut(pin);
+  infraredOut.begin();
+  
+  // auxMsg[0] is how many pulses were packed.
+  // val is frequency
+  infraredOut.sendRaw(pulseArray, auxMsg[0], val);
+}
+#endif

data/src/lib/DenkoLEDArray.cpp

@@ -0,0 +1,49 @@
+//
+// Write addressable LEDs using standard Arduino libraries.
+//
+#include "Denko.h"
+#ifdef DENKO_LED_ARRAY
+
+//
+// WS2812 / NeoPixel support using Adafruit library from:
+// https://github.com/adafruit/Adafruit_NeoPixel
+//
+# ifdef DENKO_LED_WS2812
+  #include <Adafruit_NeoPixel.h>
+#endif
+
+//
+// CMD = 19
+// Write data to a WS2812 LED array. Will generalize to other types later.
+//
+// pin          = Microcontroller pin connected to Data In pin of the LED array.
+// val          = Number of RGB or RGBW LEDS (aka pixels) in the array.
+// auxMsg[0..3] = Reserved for future settings.
+// auxMsg[4+]   = Raw pixel data, already in correct byte order (GRB, RGB, etc.).
+//
+void Denko::showLEDArray() {
+  // Setup a new LED array object.
+  Adafruit_NeoPixel ledArray(val, pin, NEO_GRB + NEO_KHZ800);
+  ledArray.begin();
+
+  // Copy LED data into the pixel buffer.
+  // NOTE: val * 3 will have to change to val * 4 for RGBW leds.
+  memcpy(ledArray.getPixels(), &auxMsg[4], val * 3);
+
+  // I have NO idea what causes the first green LED to stay stuck on.
+  // A small delay here seems like a consistent fix. ESP32 doesn't need it.
+  // AVR can get away with 32us, but others need 64.
+  #if defined(ESP8266) || defined(__SAMD21G18A__) || defined(__SAM3X8E__)
+    delayMicroseconds(64);
+  #endif
+  #ifdef __AVR__
+    delayMicroseconds(32);
+  #endif
+
+  // Write the pixel buffer to the array.
+  ledArray.show();
+
+  // Tell the computer to resume sending data.
+  sendReady();
+}
+#endif

data/src/lib/DenkoOneWire.cpp

@@ -0,0 +1,156 @@
+//
+// This file contains low-level functions to get precise (enough)
+// timing required for using the Dallas/Maxim 1-Wire protocol.
+// Higher level logic is handled in Ruby.
+//
+#include "Denko.h"
+#ifdef DENKO_ONE_WIRE
+
+// CMD = 41
+// Reset the OneWire bus and return a presence value only if requested.
+//
+void Denko::owReset(){
+  byte present;
+  pinMode(pin, OUTPUT);
+  digitalWrite(pin, LOW);
+  microDelay(500);
+  pinMode(pin, INPUT);
+  digitalWrite(pin, HIGH); // Essential on ESP32. Enables pullup on others.
+  microDelay(80);
+  present = digitalRead(pin);
+  microDelay(420);
+  if(val>0) coreResponse(pin, present);
+}
+
+// CMD = 42
+// Read a 64-bit address and complement, echoing each address bit to the bus,
+// unless we are searching a branch that has that bit set to 1, then echo 1.
+//
+void Denko::owSearch(){
+  byte addr = 0x00;
+  byte comp = 0x00;
+
+  // Start streaming the message.
+  stream->print(pin); stream->print(':');
+
+  for(byte i=0; i<8; i++){
+    for(byte j=0; j<8; j++){
+      bitWrite(addr, j, owReadBit());
+      bitWrite(comp, j, owReadBit());
+
+      // First 8 bytes of auxMsg is a 64-bit branch mask. Any bit set to 1 says
+      // that we're searching a branch with that bit set to 1, and must force
+      // it to be 1 on this pass. Write 1 to both the address bit and the bus.
+      //
+      // We also do not change the complement bit from 0, Even though the bus
+      // said 0/0, we are sending back 1/0, hiding discrepancies we are testing,
+      // only sending those that appeared this time, which is what we care about.
+      //
+      if(bitRead(auxMsg[i], j) == 1){
+        owWriteBit(1);
+        bitWrite(addr, j, 1);
+
+      // Whether there was no "1-branch" marked for this bit, or there is no
+      // discrepancy at all, just echo address bit to the bus. We compare
+      // addr/comp remotely to find discrepancies for future passes.
+      //
+      } else {
+        owWriteBit(bitRead(addr, j));
+      }
+    }
+    stream->print(addr);
+    stream->print(',');
+    stream->print(comp);
+    stream->print((i == 7) ? '\n' : ',');
+  }
+}
+
+// CMD = 43
+// Write to the OneWire bus.
+//
+// val = number of bytes to write + parasite power condition flag in MSB.
+// auxMsg[0] = first byte of data and so on...
+// Limited to 127 bytes. Validate remotely.
+//
+void Denko::owWrite(){
+  // Check and clear parasite flag.
+  bool parasite = bitRead(val, 7);
+  bitClear(val, 7);
+
+  byte b;
+  for(byte i=0; i<val; i++){
+    b = auxMsg[i];
+    for(byte j=0; j<8; j++){
+      owWriteBit(bitRead(b, j));
+    }
+  }
+
+  // Drive bus high to feed the parasite capacitor after writing if necessary.
+  if (parasite) {
+    pinMode(pin, OUTPUT);
+    digitalWrite(pin, HIGH);
+  }
+}
+
+void Denko::owWriteBit(byte b){
+  // Write slot always starts with pulling the bus low for at least 1us.
+  pinMode(pin, OUTPUT);
+  digitalWrite(pin, LOW);
+  microDelay(1);
+
+  // If 1, release so the bus idles high, and wait out the 60us write slot.
+  if(b == 1){
+    pinMode(pin, INPUT);
+    digitalWrite(pin, HIGH); // Essential on ESP32. Enables pullup on others.
+    microDelay(59);
+  // If 0, keep it low for the rest of the 60us write slot, then release.
+  } else {
+    microDelay(59);
+    pinMode(pin, INPUT);
+    digitalWrite(pin, HIGH); // Essential on ESP32. Enables pullup on others.
+  }
+  // Minimum 1us recovery time after each slot.
+  microDelay(1);
+}
+
+// CMD = 44
+// Read bytes from the OneWire bus.
+//
+// val = number of bytes to read
+//
+void Denko::owRead(){
+  byte b = 0x00;
+
+  // Start streaming the message.
+  stream->print(pin); stream->print(':');
+
+  // Print each byte read, followed by a comma, or newline for last byte.
+  for(byte i=0; i<val; i++){
+    for(byte j=0; j<8; j++){
+      bitWrite(b, j, owReadBit());
+    }
+    stream->print(b);
+    stream->print((i == (val-1)) ? '\n' : ',');
+  }
+}
+
+byte Denko::owReadBit(){
+  byte b = 0x00;
+  // Pull low for at least 1us to start a read slot, then release.
+  pinMode(pin, OUTPUT);
+  digitalWrite(pin, LOW);
+  microDelay(1);
+  pinMode(pin, INPUT);
+  digitalWrite(pin, HIGH); // Essential on ESP32. Enables pullup on others.
+
+  // Wait for the slave to write to the bus. It should hold for up to 15us.
+  microDelay(5);
+
+  // If slave pulled the bus high, the bit is a 1, else 0.
+  b = digitalRead(pin);
+
+  // Wait out the 60us read slot + recovery time.
+  microDelay(55);
+  return b;
+}
+#endif

data/src/lib/DenkoPulseInput.cpp

@@ -0,0 +1,57 @@
+//
+// Read chains of input pulses. Only used for DHT sensors right now.
+//
+#include "Denko.h"
+
+// CMD = 11
+//
+// Rapidly polls a digital input looking for rising or falling edges,
+// recording the time in microseconds between consecutive edges.
+// There is an optional reset at the beginning if the pin must be held opposite
+// to its idle state to trigger a reading.
+//
+// Max 65535 microseconds reset time.
+// Max 255 microseconds per pulse (between 2 consecutive edges).
+// Max 255 pulses counted.
+//
+// val bit 0   : whether to reset the line first or not (0 = no, 1 = yes)
+// val bit 1   : direction to pull the line (0 = low, 1 = high)
+// auxMsg[0-1] : unsigned 16-bit reset duration
+// auxMsg[2-3] : unsigned 16-bit pulse read timeout (in milliseconds)
+// auxMsg[4]   : unsigned 8-bit pulse count limit
+// auxMsg[8+]  : reserved as output buffer, will be overwritten
+//
+void Denko::pulseRead(){
+  // Reset
+  if (bitRead(val, 0)) {
+    uint16_t resetTime = (auxMsg[1] << 8) | auxMsg[0];
+    pinMode(pin, OUTPUT);
+    digitalWrite(pin, bitRead(val, 1));
+    delayMicroseconds(resetTime);
+  }
+  pinMode(pin, INPUT);
+  byte state = digitalRead(pin);
+
+  uint16_t timeout = (auxMsg[3] << 8) | auxMsg[2];
+  byte pulseCount = 0;
+
+  uint32_t start = millis();
+  uint32_t lastWrite = micros();
+
+  while ((millis() - start < timeout) && (pulseCount < auxMsg[4])) {
+    if (digitalRead(pin) != state){
+      uint32_t now = micros();
+      pulseCount++;
+      auxMsg[pulseCount+8] = now - lastWrite;
+      lastWrite = now;
+      state = state ^ 1;
+    }
+  }
+
+  stream->print(pin); stream->print(':');
+  for (byte i=1; i<=pulseCount; i++){
+    stream->print(auxMsg[i+8]);
+    stream->print((i == pulseCount) ? '\n' : ',');
+  }
+  if (pulseCount == 0) stream->print('\n');
+}

data/src/lib/DenkoSPI.cpp

@@ -0,0 +1,165 @@
+#include "Denko.h"
+
+//
+// Functions for listeners shared between hardware and bit bang SPI.
+//
+#if defined(DENKO_SPI) || defined(DENKO_SPI_BB)
+// CMD = 28
+// Send a number for a select pin to remove a SPI listener.
+void Denko::spiRemoveListener(){
+  for (int i = 0;  i < SPI_LISTENER_COUNT;  i++) {
+    if (spiListeners[i].select == pin) {
+      spiListeners[i].enabled = 0;
+    }
+  }
+}
+
+// Gets called by Denko::reset to clear all listeners.
+void Denko::spiClearListeners(){
+  for (int i = 0; i < SPI_LISTENER_COUNT; i++) {
+    spiListeners[i].enabled = 0;
+  }
+}
+
+void Denko::spiUpdateListeners(){
+  for (byte i = 0;  i < SPI_LISTENER_COUNT;  i++){
+    switch(spiListeners[i].enabled) {
+      case 1: spiReadListener(i); break;
+      #ifdef DENKO_SPI_BB
+        case 2: spiBBreadListener(i); break;
+      #endif
+      default: break;
+    }
+  }
+}
+#endif
+
+//
+// Adds hardware SPI support if DENKO_SPI defined in DenkoDefines.h.
+//
+#ifdef DENKO_SPI
+#include <SPI.h>
+// Convenience wrapper for SPI.begin
+void Denko::spiBegin(byte settings, uint32_t clockRate) {
+  SPI.begin();
+
+  // SPI mode is the lowest 2 bits of settings.
+  byte mode = settings & 0B00000011;
+  
+  // Bit 7 of settings controls bit order. 0 = LSBFIRST, 1 = MSBFIRST.
+  if (bitRead(settings, 7) == 0) {
+    // True integer value for these macros vary by platform, so just do this.
+    switch(mode){
+      case 0: SPI.beginTransaction(SPISettings(clockRate, LSBFIRST, SPI_MODE0)); break;
+      case 1: SPI.beginTransaction(SPISettings(clockRate, LSBFIRST, SPI_MODE1)); break;
+      case 2: SPI.beginTransaction(SPISettings(clockRate, LSBFIRST, SPI_MODE2)); break;
+      case 3: SPI.beginTransaction(SPISettings(clockRate, LSBFIRST, SPI_MODE3)); break;
+    }
+  } else {
+    switch(mode){
+      case 0: SPI.beginTransaction(SPISettings(clockRate, MSBFIRST, SPI_MODE0)); break;
+      case 1: SPI.beginTransaction(SPISettings(clockRate, MSBFIRST, SPI_MODE1)); break;
+      case 2: SPI.beginTransaction(SPISettings(clockRate, MSBFIRST, SPI_MODE2)); break;
+      case 3: SPI.beginTransaction(SPISettings(clockRate, MSBFIRST, SPI_MODE3)); break;
+    }
+  }
+}
+
+// Convenience wrapper for SPI.end
+void Denko::spiEnd() {
+  SPI.endTransaction();
+  // TXRX_SPI in defined for WiFi/Ethernet sketches on AVR chips.
+  // In those cases, SPI.end() can't be called since the network hardware uses it.
+  // ESP32 doesn't like when SPI.end is called either. Might be safe to never do it.
+  #if !defined(TXRX_SPI) && defined(__AVR__)
+    SPI.end();
+  #endif
+}
+
+// CMD = 26
+// Simultaneous read from and write to an SPI device.
+//
+// Request format for SPI 2-way transfers
+// pin         = select pin
+// val         = empty
+// auxMsg[0]   = SPI settings
+//   Bit 0..1  = SPI mode
+//   Bit 2..6  = ** unused **
+//   Bit 7     = Read and write bit order: MSBFIRST(1) or LSBFIRST(0)
+// auxMsg[1]   = read length  (number of bytes)
+// auxMsg[2]   = write length (number of bytes)
+// auxMsg[3-6] = clock frequency (uint32_t as 4 bytes)
+// auxMsg[7+]  = data (bytes) (write only)
+//
+void Denko::spiTransfer(uint32_t clockRate, uint8_t select, uint8_t settings, uint8_t rLength, uint8_t wLength, byte *data) {
+  spiBegin(settings, clockRate);
+
+  // Stream read bytes as if coming from select pin.
+  if (rLength > 0) {
+    stream->print(select);
+    stream->print(':');
+  }
+
+  // Pull select pin low, treating 255 as no select pin.
+  if (select != 255) {
+    pinMode(select, OUTPUT);
+    digitalWrite(select, LOW);
+  }
+  
+  for (byte i = 0;  (i < rLength || i < wLength);  i++) {
+    byte b;
+
+    if (i < wLength) {
+      b = SPI.transfer(data[i]);
+    } else {
+      b = SPI.transfer(0x00);
+    }
+
+    if (i < rLength) {
+      // Print read byte, then a comma or \n if it's the last read byte.
+      stream->print(b);
+      stream->print((i+1 == rLength) ? '\n' : ',');
+    }
+  }
+
+  // Leave select high, treating 255 as no select pin.
+  if (select != 255) digitalWrite(select, HIGH);
+
+  spiEnd();
+}
+
+// CMD = 27
+// Start listening to a register with hardware SPI.
+void Denko::spiAddListener() {
+  // Do this since RP2040 crashes with reinterpet_cast of uint32_t.
+  uint32_t  clockRate  = (uint32_t)auxMsg[3];
+            clockRate |= (uint32_t)auxMsg[4] << 8;
+            clockRate |= (uint32_t)auxMsg[5] << 16;
+            clockRate |= (uint32_t)auxMsg[6] << 24;
+  
+  for (int i = 0;  i < SPI_LISTENER_COUNT;  i++) {
+    if (spiListeners[i].enabled == 0) {
+      spiListeners[i] = {
+        clockRate,
+        pin,        // Select pin
+        auxMsg[0],  // Settings
+        auxMsg[1],  // Read length
+        1           // Enabled = 1 sets hardware SPI listener
+      };
+      return;
+    } else {
+    // Should send some kind of error if all are in use.
+    }
+  }
+}
+
+// Called by spiUpdateListeners to read an individual hardware SPI listener.
+void Denko::spiReadListener(uint8_t i) {
+  spiTransfer(spiListeners[i].clock,
+              spiListeners[i].select,
+              spiListeners[i].settings,
+              spiListeners[i].length,
+              0,                          // 0 bytes written to output
+              &auxMsg[0]);                // Get "write" data from anywhere since not writing
+}
+#endif

data/src/lib/DenkoSPIBB.cpp

@@ -0,0 +1,156 @@
+//
+// Adds SPI bitbang functionality to the Denko class if DENKO_SPI_BB defined in DenkoDefines.h.
+//
+#include "Denko.h"
+
+#ifdef DENKO_SPI_BB
+// CMD = 21
+//
+// Request format for bit banged SPI 2-way transfers
+// pin         = select pin
+// val         = empty
+// auxMsg[0]   = SPI settings
+//   Bit 0..1  = SPI mode
+//   Bit 2..6  = ** unused **
+//   Bit 7     = Read and write bit order: MSBFIRST(1) or LSBFIRST(0)
+// auxMsg[1]   = read length  (number of bytes)
+// auxMsg[2]   = write length (number of bytes)
+// auxMsg[3]   = clock pin  (uint8)
+// auxMsg[4]   = input pin  (uint8) - Set to 255 for one-way transfer.
+// auxMsg[5]   = output pin (uint8) - Set to 255 for one-way transfer.
+// auxMsg[6]   = ** unused **
+// auxMsg[7+]  = data (bytes) (write only) - Start from 7 for parity with hardware SPI.
+//
+void Denko::spiBBtransfer( uint8_t clock, uint8_t input, uint8_t output, uint8_t select, uint8_t settings,
+                          uint8_t rLength, uint8_t wLength, byte *data) {
+
+  // Mode is the lowest 2 bits of settings.
+  uint8_t mode = settings & 0b00000011;
+
+  // Bit order is stored in the highest bit of settings.
+  uint8_t bitOrder = bitRead(settings, 7);
+
+  // Set modes for input and output pins.
+  // Use a pin number of 255 to avoid touching either.
+  if (output < 255) pinMode(output, OUTPUT);
+  if (input < 255)  pinMode(input, INPUT);
+
+  // Set idle state of clock pin based on SPI mode.
+  pinMode(clock, OUTPUT);
+  if ((mode == 0)||(mode == 1)) digitalWrite(clock, LOW);
+  if ((mode == 2)||(mode == 3)) digitalWrite(clock, LOW);
+
+  // Stream read bytes as if coming from select pin.
+  if (rLength > 0) {
+    stream->print(select);
+    stream->print(':');
+  }
+
+  // Pull select pin low, treating 255 as no select pin.
+  if (select != 255) {
+    pinMode(select, OUTPUT);
+    digitalWrite(select, LOW);
+  }
+
+  for (byte i = 0;  (i < rLength || i < wLength);  i++) {
+    byte b;
+
+    if (i < wLength) {
+      b = spiBBtransferByte(clock, input, output, select, mode, bitOrder, data[i]);
+    } else {
+      b = spiBBtransferByte(clock, input, output, select, mode, bitOrder, 0x00);
+    }
+
+    if (i < rLength) {
+      // Print read byte, then a comma or \n if it's the last read byte.
+      stream->print(b);
+      stream->print((i+1 == rLength) ? '\n' : ',');
+    }
+  }
+  
+  // Leave select high, treating 255 as no select pin.
+  if (select != 255) digitalWrite(select, HIGH);
+}
+
+//
+// Used by spiBBtransfer to transfer a single byte at a time.
+//
+byte Denko::spiBBtransferByte(uint8_t clock, uint8_t input, uint8_t output, uint8_t select, uint8_t mode, uint8_t bitOrder, byte data) {
+  // Byte to return
+  byte b = 0x00;
+  
+  // Track which of the 8 bits we're on.
+  uint8_t bitPos;
+
+  // Read and write 8 bits.
+  for (int i=0; i<8; i++) {
+    // Set which bit index is being read and written based on i and bit orders.
+    if (bitOrder == 0) { bitPos = i; } else { bitPos = 7 - i; }
+
+    // SPI MODE 0
+    if (mode == 0){
+      if (output < 255) digitalWrite(output, bitRead(data, bitPos));
+      digitalWrite(clock, HIGH);
+      if (input < 255) bitWrite(b, bitPos, digitalRead(input));
+      digitalWrite(clock, LOW);
+    }
+
+    // SPI MODE 1
+    if (mode == 1){
+      digitalWrite(clock, HIGH);
+      if (output < 255) digitalWrite(output, bitRead(data, bitPos));
+      digitalWrite(clock, LOW);
+      if (input < 255) bitWrite(b, bitPos, digitalRead(input));
+    }
+
+    // SPI MODE 2
+    if (mode == 2){
+      if (output < 255) digitalWrite(output, bitRead(data, bitPos));
+      digitalWrite(clock, LOW);
+      if (input < 255) bitWrite(b, bitPos, digitalRead(input));
+      digitalWrite(clock, HIGH);
+    }
+
+    // SPI MODE 3
+    if (mode == 3){
+      digitalWrite(clock, LOW);
+      if (output < 255) digitalWrite(output, bitRead(data, bitPos));
+      digitalWrite(clock, HIGH);
+      if (input < 255) bitWrite(b, bitPos, digitalRead(input));
+    }
+  }
+  return b;
+}
+
+// CMD = 22
+// Start listening to a register with bit bang SPI.
+void Denko::spiBBaddListener() {
+  for (int i = 0;  i < SPI_LISTENER_COUNT;  i++) {
+    // Overwrite the first disabled listener in the struct array.
+    if (spiListeners[i].enabled == 0) {
+      spiListeners[i] = {
+        ((uint32_t)(auxMsg[4] << 8) | auxMsg[3]),   // Clock: [0..7], input: [8..15]
+        pin,                                        // Select pin
+        auxMsg[0],                                  // Settings mask
+        auxMsg[1],                                  // Read length
+        2                                           // Enabled = 2 sets bit bang SPI listener
+      };
+      return;
+    } else {
+    // Should send some kind of error if all are in use.
+    }
+  }
+}
+
+// Called by spiUpdateListeners to read an individual bit bang SPI listener.
+void Denko::spiBBreadListener(uint8_t i) {
+  spiBBtransfer((spiListeners[i].clock & 0xFF),         // Clock pin is bits [0..7] of the uint32
+                ((spiListeners[i].clock >> 8) & 0xFF),  // Input pin is bits [8..15] of the uint32
+                255,                                    // Disabled output pin
+                spiListeners[i].select,                 // Select pin
+                spiListeners[i].settings,
+                spiListeners[i].length,                 // Read length
+                0,                                      // 0 bytes written to output
+                &auxMsg[0]);                            // Get "write" data from anywhere since not writing
+}
+#endif

data/src/lib/DenkoServo.cpp

@@ -0,0 +1,83 @@
+//
+// This file adds to the Denko class only if DENKO_SERVO is defined in Denko.h.
+//
+#include "Denko.h"
+#ifdef DENKO_SERVO
+
+#ifdef ESP32
+  #include <ESP32Servo.h>
+#else
+  #include <Servo.h>
+#endif
+
+// 12 servos on most boards. 6 on the ATmega168.
+// Could be up to 48 on Arduino Mega.
+#if defined (__AVR_ATmega168__)
+  #define SERVO_COUNT 6
+#elif defined(ESP32)
+  #define SERVO_COUNT 16
+#else
+  #define SERVO_COUNT 12
+#endif
+
+// Create an array of wrapper structs that link pins to servo objects.
+struct ServoWrapper{
+  byte     pin;
+  boolean  active;
+  Servo    servo;
+};
+ServoWrapper servos[SERVO_COUNT];
+
+// CMD = 08
+// Attach a free servo object to the specified pin.
+void Denko::servoToggle() {
+  // Search by pin for in use servo object, detatch and set inactive.
+  if (val == 0) {
+    for (int i = 0;  i < SERVO_COUNT;  i++) {
+      if (servos[i].pin == pin) {
+        servos[i].servo.detach();
+        servos[i].active = false;
+      }
+    }
+  }
+  // Search by pin for in use servo object, attach and set active.
+  else {
+    boolean found = false;
+    uint16_t min = (auxMsg[1] << 8) | auxMsg[0];
+    uint16_t max = (auxMsg[3] << 8) | auxMsg[2];
+
+    for (int i = 0;  i < SERVO_COUNT;  i++) {
+      if (servos[i].pin == pin) {
+        found = true;
+        servos[i].servo.attach(pin, min, max);
+        servos[i].active = true;
+        break;
+      }
+    }
+    // If it doesn't exist, use the first inactive object.
+    if (found == false) {
+      for (int i = 0;  i < SERVO_COUNT;  i++) {
+        if (servos[i].active == false) {
+          servos[i].servo.attach(pin, min, max);
+          servos[i].active = true;
+          servos[i].pin = pin;
+          break;
+        }
+      }
+    }
+  }
+}
+
+// CMD = 09
+// Write a value to the servo object.
+void Denko::servoWrite() {
+  // Find servo by pin and write value to it.
+  for (int i = 0;  i < SERVO_COUNT;  i++) {
+    if (servos[i].pin == pin) {
+      uint16_t us = (auxMsg[1] << 8) | auxMsg[0];
+      servos[i].servo.writeMicroseconds(us);
+    }
+  }
+}
+
+#endif