87 lines
2.8 KiB
C++
87 lines
2.8 KiB
C++
|
#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);
|
||
|
|
}
|