implement AFServoMotor

AdafruitPWMBoard.cpp

@@ -0,0 +1,50 @@
+#include "AdafruitPWMBoard.hpp"
+
+AdafruitPWMBoard::AdafruitPWMBoard(const uint8_t addr, const uint8_t pwmFreq):
+    hwif(addr)
+{
+    if (!hwif.begin()) {
+        good = false;
+        return;
+    }
+
+    hwif.setPWMFreq(pwmFreq);
+    good = true;
+}
+
+bool AdafruitPWMBoard::isMotorSlotOccupied(const uint8_t motorIndex) const noexcept
+{
+    if (motorIndex >= 16) {
+        return false;
+    }
+
+    return occupiedMotorSlots & (1 << motorIndex);
+}
+
+bool AdafruitPWMBoard::occupyMotorSlot(const uint8_t motorIndex) noexcept
+{
+    if (motorIndex >= 16) {
+        return false;
+    }
+
+    if (isMotorSlotOccupied(motorIndex)) {
+        return false;
+    }
+
+    occupiedMotorSlots |= 1 << motorIndex;
+    return true;
+}
+
+bool AdafruitPWMBoard::releaseMotorSlot(const uint8_t motorIndex) noexcept
+{
+    if (motorIndex >= 16) {
+        return false;
+    }
+
+    if (!isMotorSlotOccupied(motorIndex)) {
+        return false;
+    }
+
+    occupiedMotorSlots &= ~(1 << motorIndex);
+    return true;
+}