got motor running

AdafruitServoMotor.cpp

@@ -1,9 +1,10 @@
 #include "AdafruitServoMotor.hpp"
 #include "StateMachine.hpp"
+#include "defs.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;
+constexpr float MIN_DELTA_FOR_MOTION_COMMIT = 0.01f;
 
 AdafruitServoMotor::AdafruitServoMotor(
     AdafruitPWMBoard& driver,
@@ -14,9 +15,9 @@ AdafruitServoMotor::AdafruitServoMotor(
     const float minSafeAngle,
     const float maxSafeAngle,
     const float specRangeDeg,
+    const float initialPosition,
     const uint16_t minPulseLength,
-    const uint16_t maxPulseLength,
-    const float initialPosition
+    const uint16_t maxPulseLength
 ) noexcept:
     driver(driver),
     motorIndex(motorIndex),
@@ -41,7 +42,6 @@ AdafruitServoMotor::AdafruitServoMotor(
         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!");
     }
@@ -53,28 +53,24 @@ bool AdafruitServoMotor::moveTo(const float target) noexcept
         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);
+    const float hardwareTarget = virtualAngleToHardwareAngle(target);
+    currentHardwarePosition = constrain(hardwareTarget, safeHardwareMinAngle, safeHardwareMaxAngle);
+        
+    // Only move the motor if it would turn it at least n degrees 
+    if (abs(lastCommittedHardwarePosition - currentHardwarePosition) >= MIN_DELTA_FOR_MOTION_COMMIT) {
+        commitMoveImmediately();
+    }
 
-    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;
-    }
+    // True if they are the same, e.g. no clamping was applied
+    return (abs(currentHardwarePosition - hardwareTarget) < COMP_EPSILON);
 }
 
 void AdafruitServoMotor::commitMoveImmediately() noexcept
 {
+    if (!driver.isGood()) {
+        StateMachine::getInstance().raiseError("Servo motor driver board gone offline!");
+    }
+
     driver.get().setPWM(motorIndex, 0, hardwareAngleToPulse(currentHardwarePosition));
     lastCommittedHardwarePosition = currentHardwarePosition;
     return false;
@@ -82,5 +78,8 @@ void AdafruitServoMotor::commitMoveImmediately() noexcept
 
 uint16_t AdafruitServoMotor::hardwareAngleToPulse(const float hardwareAngle) const noexcept
 {
-    return map(hardwareAngle, 0, specRangeDeg, minPulseLength, maxPulseLength);
+    const float t = hardwareAngle / specRangeDeg;
+    const uint16_t pulse = minPulseLength + t * (maxPulseLength - minPulseLength);
+
+    return pulse;
 }