Implement function to verify if a given angle is within the allowed range of motion for the motor.

Author: aentinger

src/lib/motors/SmartServo.cpp

@@ -180,7 +180,11 @@ int SmartServoClass::begin() {
   }
 }
 
-void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed) {
+void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed)
+{
+  if (!isValidAngle(angle))
+    return;
+
   mutex.lock();
   if (isValidId(id))
   {

src/lib/motors/SmartServo.h

@@ -89,6 +89,7 @@ class SmartServoClass
   static int constexpr MIN_MOTOR_ID = 1;
   static int constexpr MAX_MOTOR_ID = 6;
 
+  inline bool isValidAngle(float const angle) { return ((angle >= 0.0f) && (angle <= MAX_ANGLE)); }
   inline bool isValidId(int const id) const { return ((id >= MIN_MOTOR_ID) && (id <= MAX_MOTOR_ID)); }
   inline int  idToArrayIndex(int const id) const { return (id - 1); }