From d339f5b297a4a6abf2a5ba12d82a803aab589d6e Mon Sep 17 00:00:00 2001 From: Leonetienne Date: Sun, 14 Dec 2025 15:02:55 +0100 Subject: [PATCH] implement AFServoMotor --- AdafruitPWMBoard.cpp | 50 ++++++++++++++++++++++++ AdafruitPWMBoard.hpp | 27 +++++++++++++ AdafruitServoMotor.cpp | 86 ++++++++++++++++++++++++++++++++++++++++++ AdafruitServoMotor.hpp | 39 +++++++++++++++++++ RangedMotor.hpp | 2 +- 5 files changed, 203 insertions(+), 1 deletion(-) create mode 100755 AdafruitPWMBoard.cpp create mode 100755 AdafruitPWMBoard.hpp create mode 100644 AdafruitServoMotor.cpp create mode 100644 AdafruitServoMotor.hpp diff --git a/AdafruitPWMBoard.cpp b/AdafruitPWMBoard.cpp new file mode 100755 index 0000000..ee461d4 --- /dev/null +++ b/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; +} diff --git a/AdafruitPWMBoard.hpp b/AdafruitPWMBoard.hpp new file mode 100755 index 0000000..03e1f7d --- /dev/null +++ b/AdafruitPWMBoard.hpp @@ -0,0 +1,27 @@ +#pragma once +#include +#include + +// 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; +}; diff --git a/AdafruitServoMotor.cpp b/AdafruitServoMotor.cpp new file mode 100644 index 0000000..372f1b8 --- /dev/null +++ b/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); +} diff --git a/AdafruitServoMotor.hpp b/AdafruitServoMotor.hpp new file mode 100644 index 0000000..9c4bbfa --- /dev/null +++ b/AdafruitServoMotor.hpp @@ -0,0 +1,39 @@ +#pragma once +#include +#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; +}; diff --git a/RangedMotor.hpp b/RangedMotor.hpp index 267633d..5a7fe0c 100755 --- a/RangedMotor.hpp +++ b/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;