feat: create RangedMotor base class

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