implement AFServoMotor

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