Fix setMinAngle/setMaxAngle by limiting positions rather than angles.

Author: aentinger

src/Braccio++.h

@@ -255,4 +255,4 @@ inline void attachInterrupt(pin_size_t interruptNum, mbed::Callback<void()> func
   attachInterruptParam(interruptNum, callback, mode, (void*)a);
 }
 
-#endif //__BRACCIO_PLUSPLUS_H__
+#endif //__BRACCIO_PLUSPLUS_H__

src/lib/motors/SmartServo.cpp

@@ -299,27 +299,31 @@ void  SmartServoClass::setStallProtectionTime(uint8_t const id, uint8_t const ti
   mutex.unlock();
 }
 
-void  SmartServoClass::setMinAngle(float const angle) {
+void SmartServoClass::setMinAngle(uint16_t const min_angle)
+{
   mutex.lock();
-  writeWordCmd(BROADCAST, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), angleToPosition(angle));
+  writeWordCmd(BROADCAST, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), min_angle);
   mutex.unlock();
 }
 
-void  SmartServoClass::setMinAngle(uint8_t const id, float const angle) {
+void SmartServoClass::setMinAngle(uint8_t const id, uint16_t const min_angle)
+{
   mutex.lock();
-  writeWordCmd(id, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), angleToPosition(angle));
+  writeWordCmd(id, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), min_angle);
   mutex.unlock();
 }
 
-void  SmartServoClass::setMaxAngle(float const angle) {
+void SmartServoClass::setMaxAngle(uint16_t const max_angle)
+{
   mutex.lock();
-  writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), angleToPosition(angle));
+  writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), max_angle);
   mutex.unlock();
 }
 
-void  SmartServoClass::setMaxAngle(uint8_t const id, float const angle) {
+void SmartServoClass::setMaxAngle(uint8_t const id, uint16_t const max_angle)
+{
   mutex.lock();
-  writeWordCmd(id, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), angleToPosition(angle));
+  writeWordCmd(id, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), max_angle);
   mutex.unlock();
 }
 
@@ -340,4 +344,4 @@ void  SmartServoClass::getInfo(Stream & stream, uint8_t const id) {
   for (i = 0; i < sizeof(regs); i++) {
     stream.println(String(i, HEX) + " : " + String(regs[i], HEX));
   }
-}
+}

src/lib/motors/SmartServo.h

@@ -50,13 +50,10 @@ class SmartServoClass
 
   void setStallProtectionTime(uint8_t const id, uint8_t const time);
 
-  void setMinAngle(float const angle);
-
-  void setMinAngle(uint8_t const id, float const angle);
-
-  void setMaxAngle(float const angle);
-
-  void setMaxAngle(uint8_t const id, float const angle);
+  void setMinAngle(uint16_t const min_angle);
+  void setMinAngle(uint8_t const id, uint16_t const min_angle);
+  void setMaxAngle(uint16_t const max_angle);
+  void setMaxAngle(uint8_t const id, uint16_t const max_angle);
 
   void setTime(uint8_t const id, uint16_t const time);
 
@@ -131,4 +128,4 @@ class SmartServoClass
   rtos::Mutex mutex;
 };
 
-#endif // _SMARTMOTOR_H_
+#endif // _SMARTMOTOR_H_