Move implementation BraccioClass::moveTo/positions(1/2) from header to source file.

Author: aentinger

src/Braccio++.cpp

@@ -164,12 +164,42 @@ bool BraccioClass::begin(voidFuncPtr customMenu) {
   return true;
 }
 
-inline MotorsWrapper BraccioClass::move(int const id)
+MotorsWrapper BraccioClass::move(int const id)
 {
   MotorsWrapper wrapper(servos, id);
   return wrapper;
 }
 
+void BraccioClass::moveTo(float const a1, float const a2, float const a3, float const a4, float const a5, float const a6)
+{
+  servos.setPositionMode(PositionMode::SYNC);
+  servos.setPosition(1, a1, runTime);
+  servos.setPosition(2, a2, runTime);
+  servos.setPosition(3, a3, runTime);
+  servos.setPosition(4, a4, runTime);
+  servos.setPosition(5, a5, runTime);
+  servos.setPosition(6, a6, runTime);
+  servos.synchronize();
+  servos.setPositionMode(PositionMode::IMMEDIATE);
+}
+
+void BraccioClass::positions(float * buffer)
+{
+  for (int id = SmartServoClass::MIN_MOTOR_ID; id <= SmartServoClass::MAX_MOTOR_ID; id++)
+    *buffer++ = servos.getPosition(id);
+}
+
+void BraccioClass::positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6)
+{
+  // TODO: add check if motors are actually connected
+  a1 = servos.getPosition(1);
+  a2 = servos.getPosition(2);
+  a3 = servos.getPosition(3);
+  a4 = servos.getPosition(4);
+  a5 = servos.getPosition(5);
+  a6 = servos.getPosition(6);
+}
+
 void BraccioClass::connectJoystickTo(lv_obj_t* obj) {
   lv_group_add_obj(p_objGroup, obj);
   lv_indev_set_group(kb_indev, p_objGroup);

src/Braccio++.h

@@ -91,32 +91,10 @@ class BraccioClass
          MotorsWrapper move(int const id);
   inline MotorsWrapper get(int const id) { return move(id); }
 
-  void moveTo(float a1, float a2, float a3, float a4, float a5, float a6) {
-    servos.setPositionMode(PositionMode::SYNC);
-    servos.setPosition(1, a1, runTime);
-    servos.setPosition(2, a2, runTime);
-    servos.setPosition(3, a3, runTime);
-    servos.setPosition(4, a4, runTime);
-    servos.setPosition(5, a5, runTime);
-    servos.setPosition(6, a6, runTime);
-    servos.synchronize();
-    servos.setPositionMode(PositionMode::IMMEDIATE);
-  }
-  // getters
-  void positions(float* buffer) {
-    for (int i = 1; i < 7; i++)  {
-      *buffer++ = servos.getPosition(i);
-    }
-  }
-  void positions(float& a1, float& a2, float& a3, float& a4, float& a5, float& a6) {
-    // TODO: add check if motors are actually connected
-    a1 = servos.getPosition(1);
-    a2 = servos.getPosition(2);
-    a3 = servos.getPosition(3);
-    a4 = servos.getPosition(4);
-    a5 = servos.getPosition(5);
-    a6 = servos.getPosition(6);
-  }
+  void moveTo(float const a1, float const a2, float const a3, float const a4, float const a5, float const a6);
+  void positions(float * buffer);
+  void positions(float & a1, float & a2, float & a3, float & a4, float & a5, float & a6);
+
   float position(int joint_index);
   bool connected(int joint_index) {
     return _connected[joint_index];

src/lib/motors/SmartServo.h

@@ -71,7 +71,9 @@ class SmartServoClass
 
   inline int getErrors() const { return _errors; }
 
-  static const int BROADCAST = 0xFE;
+  static int   constexpr BROADCAST = 0xFE;
+  static int   constexpr MIN_MOTOR_ID = 1;
+  static int   constexpr MAX_MOTOR_ID = 6;
   static float constexpr MAX_ANGLE = 315.0f;
 
 private:
@@ -81,9 +83,6 @@ class SmartServoClass
   static int constexpr MAX_RX_PAYLOAD_LEN = 10;
   static int constexpr MAX_POSITION = 4000;
 
-  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); }