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

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

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)