diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
index 450c8d2..88a0db9 100644
--- a/src/Braccio++.cpp
+++ b/src/Braccio++.cpp
@@ -164,6 +164,42 @@ bool BraccioClass::begin(voidFuncPtr customMenu) {
   return true;
 }
 
+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);
diff --git a/src/Braccio++.h b/src/Braccio++.h
index 117d90e..8589add 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -87,47 +87,15 @@ class BraccioClass
   inline bool begin() { return begin(nullptr); }
          bool begin(voidFuncPtr customMenu);
 
-  // setters
-  MotorsWrapper move(int joint_index) {
-    MotorsWrapper wrapper(servos, joint_index);
-    return wrapper;
-  }
-  MotorsWrapper get(int joint_index) {
-    return move(joint_index);
-  }
-  void moveTo(int joint_index, int position) {
-    //servos.setPosition(joint_index, position, 100);
-  }
-  void moveTo(int joint_index, float angle) {
-    servos.setPosition(joint_index, angle, 100);
-  }
-  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);
-  }
-  float position(int joint_index);
+
+         MotorsWrapper move(int const id);
+  inline MotorsWrapper get(int const id) { return move(id); }
+
+  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);
+
+
   bool connected(int joint_index) {
     return _connected[joint_index];
   }
diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h
index c0a8125..63914af 100644
--- a/src/lib/motors/SmartServo.h
+++ b/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); }