diff --git a/RangedMotor.cpp b/RangedMotor.cpp new file mode 100755 index 0000000..d3e8fba --- /dev/null +++ b/RangedMotor.cpp @@ -0,0 +1,133 @@ +#include "RangedMotor.hpp" +#include "StateMachine.hpp" +#include "defs.hpp" + +RangedMotor::RangedMotor( + const float gearboxRatio, + const float scaleCalibration, + const float zeroOffset, + const float minSafeAngle, + const float maxSafeAngle +) noexcept +{ + // Using setters for validation + setGearboxRatio(gearboxRatio); + setScaleCalibration(scaleCalibration); + setZeroOffset(zeroOffset); + setSafeMinAngle(minSafeAngle); + setSafeMaxAngle(maxSafeAngle); +} + +// Getters +float RangedMotor::getGearboxRatio() const noexcept +{ + return gearboxRatio; +} + +float RangedMotor::getScaleCalibration() const noexcept +{ + return scaleCalibration; +} + +float RangedMotor::getZeroOffset() const noexcept +{ + return zeroOffset; +} + +float RangedMotor::getSafeMinAngle() const noexcept +{ + return hardwareAngleToVirtualAngle(safeHardwareMinAngle); +} + +float RangedMotor::getSafeMaxAngle() const noexcept +{ + return hardwareAngleToVirtualAngle(safeHardwareMaxAngle); +} + +float RangedMotor::getSafeHardwareMinAngle() const noexcept +{ + return safeHardwareMinAngle; +} + +float RangedMotor::getSafeHardwareMaxAngle() const noexcept +{ + return safeHardwareMaxAngle; +} + +// Setters +void RangedMotor::setGearboxRatio(float gearboxRatio) noexcept +{ + if (abs(gearboxRatio) < COMP_EPSILON) { + StateMachine::getInstance().raiseError("Zero-gearbox-ratio was set on ranged motor!"); + return; + } + this->gearboxRatio = gearboxRatio; +} + +void RangedMotor::setScaleCalibration(float scaleCalibration) noexcept +{ + if (abs(gearboxRatio) < COMP_EPSILON) { + StateMachine::getInstance().raiseError("Zero-scale-calibration was set on ranged motor!"); + return; + } + this->scaleCalibration = scaleCalibration; +} + +void RangedMotor::setZeroOffset(float zeroOffset) noexcept +{ + this->zeroOffset = zeroOffset; +} + +void RangedMotor::setSafeMinAngle(float safeMinAngle) noexcept +{ + safeHardwareMinAngle = virtualAngleToHardwareAngle(safeMinAngle); +} + +void RangedMotor::setSafeMaxAngle(float safeMaxAngle) noexcept +{ + safeHardwareMaxAngle = virtualAngleToHardwareAngle(safeMaxAngle); +} + +void RangedMotor::setSafeHardwareMinAngle(float safeHardwareMinAngle) noexcept +{ + this->safeHardwareMinAngle = safeHardwareMinAngle; +} + +void RangedMotor::setSafeHardwareMaxAngle(float safeHardwareMaxAngle) noexcept +{ + this->safeHardwareMaxAngle = safeHardwareMaxAngle; +} + +float RangedMotor::virtualAngleToHardwareAngle(float virtualAngle) const noexcept +{ + // Apply scaling correction + virtualAngle *= scaleCalibration; + + // Apply offset + virtualAngle += zeroOffset; + + // Apply gearbox reduction + virtualAngle *= gearboxRatio; + + // Return the now no-longer virtual angle + return virtualAngle; +} + +float RangedMotor::hardwareAngleToVirtualAngle(float hardwareAngle) const noexcept +{ + // Undo gearbox ration + hardwareAngle /= gearboxRatio; + + // Undo offset + hardwareAngle -= zeroOffset; + + // Undo scaling correction + hardwareAngle /= scaleCalibration; + + return hardwareAngle; +} + +float RangedMotor::calculateScaleCalibration(const float virtualRange, const float hardwareRange) +{ + return virtualRange / hardwareRange; +} diff --git a/RangedMotor.hpp b/RangedMotor.hpp new file mode 100755 index 0000000..267633d --- /dev/null +++ b/RangedMotor.hpp @@ -0,0 +1,51 @@ +#pragma once + +// Abstract class +class RangedMotor +{ +public: + RangedMotor( + const float gearboxRatio, + const float scaleCalibration, + const float zeroOffset, + const float minSafeAngle, + const float maxSafeAngle + ) noexcept; + + virtual bool moveTo(float target) noexcept; + virtual float getPosition() const noexcept; + + float getGearboxRatio() const noexcept; + float getScaleCalibration() const noexcept; + float getZeroOffset() const noexcept; + float getSafeMinAngle() const noexcept; + float getSafeMaxAngle() const noexcept; + float getSafeHardwareMinAngle() const noexcept; + float getSafeHardwareMaxAngle() const noexcept; + + void setGearboxRatio(float gearboxRatio) noexcept; + void setScaleCalibration(float scaleCalibration) noexcept; + void setZeroOffset(float zeroOffset) noexcept; + void setSafeMinAngle(float safeMinAngle) noexcept; + void setSafeMaxAngle(float safeMaxAngle) noexcept; + void setSafeHardwareMinAngle(float safeHardwareMinAngle) noexcept; + void setSafeHardwareMaxAngle(float safeHardwareMaxAngle) noexcept; + + + // Will translate virtual-space-angles to hardware-space-angles + float virtualAngleToHardwareAngle(float virtualAngle) const noexcept; + // Will translate hardware-space-angles to virtual-space-angles + float hardwareAngleToVirtualAngle(float hardwareAngle) const noexcept; + +protected: + // Calculates a command-space scale compensation factor. + // The returned value is multiplied with virtual angle so that + // the motors actual movement matches the intended movement. + static float calculateScaleCalibration(const float virtualRange, const float hardwareRange); + + float gearboxRatio; // Gearbox ratio + float scaleCalibration; // Scale calibration + float zeroOffset; // Zero offset calibration + float safeHardwareMinAngle; // Absolute hardware space minimum + float safeHardwareMaxAngle; // Absolute hardware space maximum +}; diff --git a/defs.hpp b/defs.hpp index fa616c1..1876ab2 100755 --- a/defs.hpp +++ b/defs.hpp @@ -1,5 +1,7 @@ #pragma once +constexpr float COMP_EPSILON = 0.01; + // ----- Servo config ----- #define SERVO_PMIN 125 // Minimum pulse length (0 degrees) #define SERVO_PMAX 625 // Maximum pulse length (180 degrees)