add support for servo offset

.vscode/settings.json

@@ -37,6 +37,10 @@
         "random": "cpp",
         "mutex": "cpp",
         "optional": "cpp",
-        "__tree": "cpp"
+        "__tree": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "numeric": "cpp",
+        "utility": "cpp"
     }
 }

platformio.ini

@@ -3,6 +3,6 @@ platform = native
 test_build_src = yes
 build_flags =
 	-Wno-deprecated-declarations
-debug_test = animation/test_scene_callback
+debug_test = test_servo
 lib_deps = 
 	ArduinoFake

src/internal/Animation.cpp

@@ -66,6 +66,10 @@ void Animation::setServoThreshold(byte id, byte value) {
   this->servoManager.setThreshold(id, value);
 }
 
+void Animation::setServoOffset(byte id, int offset) {
+  this->servoManager.setOffset(id, offset);
+}
+
 void Animation::setScene(byte index) {
   if (!this->scenes || index >= this->addIndex) {
     return;

src/internal/Animation.h

@@ -43,6 +43,7 @@ class Animation {
   void live(AnimationData &data);
   void setDefaultServoThreshold(byte value);
   void setServoThreshold(byte id, byte value);
+  void setServoOffset(byte id, int offset);
 
   bool hasFinished();
   bool hasScenes();

src/internal/Servo.cpp

@@ -10,7 +10,11 @@ Servo::Servo(byte id, pcb positionCallback, byte threshold) {
   this->setThreshold(threshold);
 }
 
-void Servo::move(int position) {
+void Servo::move(int position, bool useOffset) {
+  if (useOffset && this->offset != 0) {
+    position += this->offset;
+  }
+
   if (position == this->currentPosition ||
       this->positionExceedsThreshold(position)) {
     return;
@@ -30,7 +34,7 @@ void Servo::moveTowardsNeutral() {
   }
 
   if (this->threshold == 0) {
-    return this->move(this->neutralPosition);
+    return this->move(this->neutralPosition, false);
   }
 
   int newPosition = this->currentPosition;
@@ -43,7 +47,7 @@ void Servo::moveTowardsNeutral() {
     newPosition += this->step;
   }
 
-  this->move(newPosition);
+  this->move(newPosition, false);
 }
 
 bool Servo::isNeutral() {
@@ -59,6 +63,10 @@ void Servo::setThreshold(byte value) {
   this->step = round(value / STEP_DIVIDER);
 }
 
+void Servo::setOffset(int offset) {
+  this->offset = offset;
+}
+
 bool Servo::positionExceedsThreshold(int position) {
   if (this->currentPosition == -1) {
     return false;

src/internal/Servo.h

@@ -11,9 +11,10 @@ class Servo {
 public:
   Servo(byte id, pcb positionCallback, byte threshold = 0);
 
-  void move(int position);
+  void move(int position, bool useOffset = true);
   void moveTowardsNeutral();
   void setThreshold(byte value);
+  void setOffset(int offset);
   void setPositionCallback(pcb positionCallback);
 
   bool isNeutral();
@@ -29,6 +30,7 @@ class Servo {
 
   int neutralPosition = -1;
   int currentPosition = -1;
+  int offset = 0;
 
   pcb positionCallback = nullptr;
 

src/internal/ServoManager.cpp

@@ -25,13 +25,15 @@ void ServoManager::setDefaultThreshold(byte value) {
 void ServoManager::setThreshold(byte servoId, byte value) {
   Servo *servo = this->getServo(servoId);
 
-  if (!servo) {
-    servo = this->addServo(servoId);
-  }
-
   servo->setThreshold(value);
 }
 
+void ServoManager::setOffset(byte servoId, int offset) {
+  Servo *servo = this->getServo(servoId);
+
+  servo->setOffset(offset);
+}
+
 void ServoManager::parseData(AnimationData *data, bool considerLineBreaks) {
   if (!data || !this->positionCallback) {
     return;
@@ -57,13 +59,8 @@ void ServoManager::handleCommand() {
 
   byte id = this->command.getServoID();
   int position = this->command.getServoPosition();
-
   Servo *servo = this->getServo(id);
 
-  if (!servo) {
-    servo = this->addServo(id);
-  }
-
   servo->move(position);
 }
 
@@ -116,5 +113,5 @@ Servo *ServoManager::getServo(byte id) {
     }
   }
 
-  return nullptr;
+  return this->addServo(id);
 }

src/internal/ServoManager.h

@@ -17,6 +17,7 @@ class ServoManager {
   void setPositionCallback(pcb positionCallback);
   void setDefaultThreshold(byte value);
   void setThreshold(byte servoId, byte value);
+  void setOffset(byte servoId, int offset);
   void parseData(AnimationData *data, bool considerLineBreaks = true);
   void moveAllTowardsNeutral();
 

test/test_servo/test_servo.cpp

@@ -83,10 +83,66 @@ void test_threshold(void) {
   TEST_ASSERT_EQUAL(2, logIndex);
 }
 
+void test_offset(void) {
+  byte servoId = 3;
+  Servo servo(servoId, move);
+  servo.setOffset(10);
+
+  TEST_ASSERT_TRUE(servo.isNeutral());
+
+  servo.move(340);
+
+  TEST_ASSERT_EQUAL(servoId, positions[0].servoId);
+  TEST_ASSERT_EQUAL(350, positions[0].position);
+
+  servo.move(330);
+
+  TEST_ASSERT_EQUAL(servoId, positions[1].servoId);
+  TEST_ASSERT_EQUAL(340, positions[1].position);
+
+  servo.setOffset(-10);
+  servo.move(320);
+
+  TEST_ASSERT_EQUAL(servoId, positions[2].servoId);
+  TEST_ASSERT_EQUAL(310, positions[2].position);
+
+  TEST_ASSERT_EQUAL(3, logIndex);
+}
+
+void test_move_towards_neutral_with_offset(void) {
+  byte servoId = 123;
+  Servo servo(servoId, move, 10);
+  servo.setOffset(10);
+
+  servo.move(350);
+
+  TEST_ASSERT_EQUAL(servoId, positions[0].servoId);
+  TEST_ASSERT_EQUAL(360, positions[0].position);
+  TEST_ASSERT_TRUE(servo.isNeutral());
+
+  servo.move(340);
+
+  TEST_ASSERT_EQUAL(servoId, positions[1].servoId);
+  TEST_ASSERT_EQUAL(350, positions[1].position);
+  TEST_ASSERT_FALSE(servo.isNeutral());
+
+  for (int i = 0; i < 10; i++) {
+    servo.moveTowardsNeutral();
+
+    TEST_ASSERT_EQUAL(servoId, positions[2 + i].servoId);
+    TEST_ASSERT_EQUAL(351 + i, positions[2 + i].position);
+  }
+
+  TEST_ASSERT_TRUE(servo.isNeutral());
+  TEST_ASSERT_EQUAL(12, logIndex);
+}
+
 int main(int argc, char **argv) {
   UNITY_BEGIN();
-  RUN_TEST(test_move);
-  RUN_TEST(test_move_towards_neutral);
-  RUN_TEST(test_threshold);
+  // RUN_TEST(test_move);
+  // RUN_TEST(test_move_towards_neutral);
+  // RUN_TEST(test_threshold);
+  // RUN_TEST(test_offset);
+  RUN_TEST(test_move_towards_neutral_with_offset);
   UNITY_END();
 }