From 95771ed26486ef5de79124e15e46564802d5fe31 Mon Sep 17 00:00:00 2001 From: Tim Hendriks Date: Sat, 20 Jul 2024 14:29:31 +0200 Subject: [PATCH] add support for servo offset --- .vscode/settings.json | 6 +++- platformio.ini | 2 +- src/internal/Animation.cpp | 4 +++ src/internal/Animation.h | 1 + src/internal/Servo.cpp | 14 ++++++-- src/internal/Servo.h | 4 ++- src/internal/ServoManager.cpp | 17 ++++------ src/internal/ServoManager.h | 1 + test/test_servo/test_servo.cpp | 62 ++++++++++++++++++++++++++++++++-- 9 files changed, 92 insertions(+), 19 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 6bfc61c..c3bb3ae 100644 --- a/.vscode/settings.json +++ b/.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" } } \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 22dff0d..c28515f 100644 --- a/platformio.ini +++ b/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 diff --git a/src/internal/Animation.cpp b/src/internal/Animation.cpp index 118cea7..a3f2184 100644 --- a/src/internal/Animation.cpp +++ b/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; diff --git a/src/internal/Animation.h b/src/internal/Animation.h index 6eb67d7..42da630 100644 --- a/src/internal/Animation.h +++ b/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(); diff --git a/src/internal/Servo.cpp b/src/internal/Servo.cpp index 083f1a6..dabddeb 100644 --- a/src/internal/Servo.cpp +++ b/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; diff --git a/src/internal/Servo.h b/src/internal/Servo.h index e6c02de..4d5cf2b 100644 --- a/src/internal/Servo.h +++ b/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; diff --git a/src/internal/ServoManager.cpp b/src/internal/ServoManager.cpp index 3fe10cb..b414466 100644 --- a/src/internal/ServoManager.cpp +++ b/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); } diff --git a/src/internal/ServoManager.h b/src/internal/ServoManager.h index 2da4c24..928c389 100644 --- a/src/internal/ServoManager.h +++ b/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(); diff --git a/test/test_servo/test_servo.cpp b/test/test_servo/test_servo.cpp index 14f83cc..008ed62 100644 --- a/test/test_servo/test_servo.cpp +++ b/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(); } \ No newline at end of file