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;
+}

AdafruitPWMBoard.hpp

@@ -0,0 +1,27 @@
+#pragma once
+#include <Arduino.h>
+#include <Adafruit_PWMServoDriver.h>
+
+// Wrapper class to prevent duplicate motor port assignments
+// This board can drive up to 16 motors
+class AdafruitPWMBoard
+{
+    public:
+        AdafruitPWMBoard(const uint8_t addr, const uint8_t pwmFreq);
+
+        Adafruit_PWMServoDriver& get() noexcept { return hwif; };
+        const Adafruit_PWMServoDriver& get() const noexcept { return hwif; };
+        bool isGood() const noexcept { return good; }
+        bool isMotorSlotOccupied(const uint8_t motorIndex) const noexcept;
+        // Returns false if the slot is already occupied or index is out of range
+        bool occupyMotorSlot(const uint8_t motorIndex) noexcept;
+        // Returns false if the slot is already free or index is out of range
+        bool releaseMotorSlot(const uint8_t motorIndex) noexcept;
+
+
+    private:
+        // No release needed, hardware interface does not expose any method for release
+        Adafruit_PWMServoDriver hwif;
+        bool good;
+        uint16_t occupiedMotorSlots = 0;
+};

AdafruitServoMotor.cpp

@@ -0,0 +1,86 @@
+#include "AdafruitServoMotor.hpp"
+#include "StateMachine.hpp"
+
+// Motor hardware angle must be at least changed by that much before
+// the motion is commited to the motor (prevent motor rattle)
+constexpr float MIN_DELTA_FOR_MOTION_COMMIT = 0.02f;
+
+AdafruitServoMotor::AdafruitServoMotor(
+    AdafruitPWMBoard& driver,
+    const uint8_t motorIndex,
+    const float gearboxRatio,
+    const float scaleCalibration,
+    const float zeroOffset,
+    const float minSafeAngle,
+    const float maxSafeAngle,
+    const float specRangeDeg,
+    const uint16_t minPulseLength,
+    const uint16_t maxPulseLength,
+    const float initialPosition
+) noexcept:
+    driver(driver),
+    motorIndex(motorIndex),
+    RangedMotor(
+        gearboxRatio,
+        scaleCalibration,
+        zeroOffset,
+        minSafeAngle,
+        maxSafeAngle
+    ),
+    currentHardwarePosition      (virtualAngleToHardwareAngle(initialPosition)),
+    lastCommittedHardwarePosition(virtualAngleToHardwareAngle(initialPosition)),
+    specRangeDeg(specRangeDeg),
+    minPulseLength(minPulseLength),
+    maxPulseLength(maxPulseLength)
+{
+    if (motorIndex >= 16) {
+        StateMachine::getInstance().raiseError("Servo motor index out of range for adafruit pwm board!");
+    }
+
+    if (!driver.isGood()) {
+        StateMachine::getInstance().raiseError("Servo motor received offline driver board!");
+    }
+
+
+    if (!driver.occupyMotorSlot(motorIndex)) {
+        StateMachine::getInstance().raiseError("Servo motor index already occupied for adafruit pwm board!");
+    }
+}
+
+bool AdafruitServoMotor::moveTo(const float target) noexcept
+{
+    if (!driver.isGood()) {
+        StateMachine::getInstance().raiseError("Servo motor driver board gone offline!");
+    }
+
+    // delta = abs(old_hardware_pos - new_hardware_pos)
+    float delta = currentHardwarePosition;
+    currentHardwarePosition = virtualAngleToHardwareAngle(target);
+    delta = abs(delta - currentHardwarePosition);
+
+    if (currentHardwarePosition >= safeHardwareMinAngle &&
+        currentHardwarePosition <= safeHardwareMaxAngle) {
+        
+        // Only move the motor if it would turn it at least n degrees 
+        if (abs(lastCommittedHardwarePosition - currentHardwarePosition) >= MIN_DELTA_FOR_MOTION_COMMIT) {
+            commitMoveImmediately();
+        }
+        
+        return true;
+    }
+    else {
+        return false;
+    }
+}
+
+void AdafruitServoMotor::commitMoveImmediately() noexcept
+{
+    driver.get().setPWM(motorIndex, 0, hardwareAngleToPulse(currentHardwarePosition));
+    lastCommittedHardwarePosition = currentHardwarePosition;
+    return false;
+}
+
+uint16_t AdafruitServoMotor::hardwareAngleToPulse(const float hardwareAngle) const noexcept
+{
+    return map(hardwareAngle, 0, specRangeDeg, minPulseLength, maxPulseLength);
+}

AdafruitServoMotor.hpp

@@ -0,0 +1,39 @@
+#pragma once
+#include <Arduino.h>
+#include "RangedMotor.hpp"
+#include "AdafruitPWMBoard.hpp"
+
+class AdafruitServoMotor : public RangedMotor
+{
+    public:
+        AdafruitServoMotor(
+            AdafruitPWMBoard& driver,
+            const uint8_t motorIndex,
+            const float gearboxRatio,
+            const float scaleCalibration,
+            const float zeroOffset,
+            const float minSafeAngle,
+            const float maxSafeAngle,
+            const float specRangeDeg, // 180 for a 180-deg-servo, 270 for a 270-deg-servo, etc ...
+            const uint16_t minPulseLength = 125,
+            const uint16_t maxPulseLength = 125,
+            const float initialPosition = 0
+        ) noexcept;
+
+        // Returns true if the motion is accepted, false if it is outside of safe bounds.
+        bool moveTo(const float target) noexcept override;
+        float getPosition() const noexcept override { return hardwareAngleToVirtualAngle(currentHardwarePosition); };
+
+        // Will apply the current position immediately, no matter how small the move might be.
+        void commitMoveImmediately() noexcept;
+
+    protected:
+        uint16_t hardwareAngleToPulse(const float hardwareAngle) const noexcept;
+        AdafruitPWMBoard& driver;
+        const uint8_t motorIndex;
+        float currentHardwarePosition = 0;
+        float lastCommittedHardwarePosition = 0;
+        float specRangeDeg;
+        const uint8_t minPulseLength;
+        const uint8_t maxPulseLength;
+};

RangedMotor.hpp

@@ -12,7 +12,7 @@ public:
         const float maxSafeAngle
     ) noexcept;
 
-    virtual bool moveTo(float target) noexcept;
+    virtual bool moveTo(const float target) noexcept;
     virtual float getPosition() const noexcept;
 
     float getGearboxRatio() const noexcept;