use threshold to calculate move to neutral steps

timhendriks93 · timhendriks93 · commit 89f51053c0f7 · 2023-04-26T21:46:44.000+02:00
diff --git a/examples/SwitchModeButton/SwitchModeButton.ino b/examples/SwitchModeButton/SwitchModeButton.ino
@@ -60,7 +60,7 @@ void modeChanged(byte prevMode, byte newMode) {
 BlenderServoAnimation::Animation animation(FPS, FRAMES);
 
 // Servo object to manage the positions
-BlenderServoAnimation::Servo myBlenderServo(0, Bone, move);
+BlenderServoAnimation::Servo myBlenderServo(0, Bone, move, 100);
 
 // Callback to be triggered on a short button press
 void onPressed() {
diff --git a/src/internal/Animation.cpp b/src/internal/Animation.cpp
@@ -45,7 +45,7 @@ void Animation::run(unsigned long currentMicros) {
     this->handlePlayMode(currentMicros);
     break;
   case MODE_STOP:
-    this->handleStopMode();
+    this->handleStopMode(currentMicros);
     break;
   case MODE_LIVE:
     this->handleLiveMode();
@@ -88,9 +88,15 @@ void Animation::handlePlayMode(unsigned long currentMicros) {
   }
 }
 
-void Animation::handleStopMode() {
+void Animation::handleStopMode(unsigned long currentMicros) {
   bool allNeutral = true;
 
+  if (currentMicros - this->lastMicros < 10000) {
+    return;
+  }
+  
+  this->lastMicros = currentMicros;
+
   for (int i = 0; i < MAX_SERVO_COUNT; i++) {
     Servo *servo = this->servos[i];
 
@@ -100,14 +106,11 @@ void Animation::handleStopMode() {
     }
   }
 
-  if (allNeutral) {
-    this->changeMode(MODE_DEFAULT);
+  if (!allNeutral) {
     return;
   }
 
-  if (this->stopStepDelay > 0) {
-    delay(this->stopStepDelay);
-  }
+  this->changeMode(MODE_DEFAULT);
 }
 
 void Animation::handleLiveMode() {
@@ -158,13 +161,13 @@ void Animation::loop(unsigned long currentMicros) {
   this->changeMode(MODE_LOOP);
 }
 
-void Animation::stop(byte stepDelay) {
+void Animation::stop(unsigned long currentMicros) {
   if (this->modeIsIn(2, MODE_DEFAULT, MODE_STOP)) {
     return;
   }
 
-  this->stopStepDelay = stepDelay;
   this->frame = 0;
+  this->lastMicros = currentMicros;
   this->changeMode(MODE_STOP);
 }
 
diff --git a/src/internal/Animation.h b/src/internal/Animation.h
@@ -17,7 +17,6 @@ class Animation {
   static const long SECOND_IN_MICROS = 1000000;
 
   byte fps = 0;
-  byte stopStepDelay = 20;
   byte mode = MODE_DEFAULT;
 
   int diffPerSecond = 0;
@@ -35,7 +34,7 @@ class Animation {
 
   void changeMode(byte mode);
   void handlePlayMode(unsigned long currentMicros);
-  void handleStopMode();
+  void handleStopMode(unsigned long currentMicros);
   void handleLiveMode();
 
 public:
@@ -56,7 +55,7 @@ class Animation {
   void play(unsigned long currentMicros = micros());
   void pause();
   void loop(unsigned long currentMicros = micros());
-  void stop(byte stepDelay = 20);
+  void stop(unsigned long currentMicros = micros());
   void live(Stream &liveStream);
 
   byte getFPS();
diff --git a/src/internal/Servo.cpp b/src/internal/Servo.cpp
@@ -40,6 +40,7 @@ void Servo::moveByFrame(int frame) {
 }
 
 void Servo::moveTowardsNeutral(bool inSteps) {
+  int step = round(this->threshold / 10);
   int newPosition = this->currentPosition;
 
   if (inSteps == false) {
@@ -48,9 +49,13 @@ void Servo::moveTowardsNeutral(bool inSteps) {
   }
 
   if (this->currentPosition > this->neutralPosition) {
-    newPosition--;
+    newPosition -= step;
   } else if (this->currentPosition < this->neutralPosition) {
-    newPosition++;
+    newPosition += step;
+  }
+
+  if (abs(this->neutralPosition - newPosition) < step) {
+    newPosition = this->neutralPosition;
   }
 
   this->move(newPosition);
diff --git a/src/internal/Show.cpp b/src/internal/Show.cpp
@@ -104,12 +104,12 @@ void Show::pause() {
   this->changeMode(MODE_PAUSE);
 }
 
-void Show::stop(byte stepDelay) {
+void Show::stop(unsigned long currentMicros) {
   if (!this->animation || this->modeIsIn(2, MODE_DEFAULT, MODE_STOP)) {
     return;
   }
 
-  this->animation->stop(stepDelay);
+  this->animation->stop(currentMicros);
   this->changeMode(MODE_STOP);
 }
 
diff --git a/src/internal/Show.h b/src/internal/Show.h
@@ -51,7 +51,7 @@ class Show {
   void playRandom(unsigned long currentMicros = micros());
   void loop(unsigned long currentMicros = micros());
   void pause();
-  void stop(byte stepDelay = 20);
+  void stop(unsigned long currentMicros = micros());
   void live(Stream &liveStream);
   void reset();
 
diff --git a/test/animation/test_live/test_live.cpp b/test/animation/test_live/test_live.cpp
@@ -93,7 +93,7 @@ void test_prevented(void) {
   TEST_ASSERT_EQUAL(Animation::MODE_LOOP, animation.getMode());
   animation.live(mock);
   TEST_ASSERT_EQUAL(Animation::MODE_LOOP, animation.getMode());
-  animation.stop();
+  animation.stop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, animation.getMode());
   animation.live(mock);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, animation.getMode());
diff --git a/test/animation/test_loop/test_loop.cpp b/test/animation/test_loop/test_loop.cpp
@@ -60,11 +60,11 @@ void test_prevented(void) {
   TEST_ASSERT_EQUAL(Animation::MODE_PLAY, animation.getMode());
   animation.loop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_PLAY, animation.getMode());
-  animation.stop();
+  animation.stop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, animation.getMode());
   animation.loop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, animation.getMode());
-  animation.run(0);
+  animation.run(10000);
   animation.live(mock);
   TEST_ASSERT_EQUAL(Animation::MODE_LIVE, animation.getMode());
   animation.loop(0);
diff --git a/test/animation/test_mode_callback/test_mode_callback.cpp b/test/animation/test_mode_callback/test_mode_callback.cpp
@@ -58,10 +58,10 @@ void test_all_modes(void) {
   animation.loop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_PAUSE, prevMode);
   TEST_ASSERT_EQUAL(Animation::MODE_LOOP, newMode);
-  animation.stop();
+  animation.stop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_LOOP, prevMode);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, newMode);
-  animation.run(0);
+  animation.run(10000);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, prevMode);
   TEST_ASSERT_EQUAL(Animation::MODE_DEFAULT, newMode);
   animation.live(mock);
diff --git a/test/animation/test_pause/test_pause.cpp b/test/animation/test_pause/test_pause.cpp
@@ -83,11 +83,11 @@ void test_prevented(void) {
   animation.pause();
   TEST_ASSERT_EQUAL(Animation::MODE_DEFAULT, animation.getMode());
   animation.play(0);
-  animation.stop();
+  animation.stop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, animation.getMode());
   animation.pause();
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, animation.getMode());
-  animation.run(0);
+  animation.run(10000);
   animation.live(mock);
   TEST_ASSERT_EQUAL(Animation::MODE_LIVE, animation.getMode());
   animation.pause();
diff --git a/test/animation/test_play/test_play.cpp b/test/animation/test_play/test_play.cpp
@@ -66,11 +66,11 @@ void test_prevented(void) {
   TEST_ASSERT_EQUAL(Animation::MODE_LOOP, animation.getMode());
   animation.play(0);
   TEST_ASSERT_EQUAL(Animation::MODE_LOOP, animation.getMode());
-  animation.stop();
+  animation.stop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, animation.getMode());
   animation.play(0);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, animation.getMode());
-  animation.run(0);
+  animation.run(10000);
   animation.live(mock);
   TEST_ASSERT_EQUAL(Animation::MODE_LIVE, animation.getMode());
   animation.play(0);
diff --git a/test/animation/test_stop/test_stop.cpp b/test/animation/test_stop/test_stop.cpp
@@ -14,9 +14,9 @@ const int positionsB[5] PROGMEM = {250, 240, 230, 240, 230};
 void test_stop(void) {
   Animation animation(FPS, FRAMES);
   Servo servos[] = {
-      Servo(0, positions, move),
-      Servo(1, positionsB, move),
-      Servo(2, move),
+      Servo(0, positions, move, 10),
+      Servo(1, positionsB, move, 10),
+      Servo(2, move, 10),
   };
   animation.addServos(servos, 3);
   TEST_ASSERT_EQUAL(Animation::MODE_DEFAULT, animation.getMode());
@@ -48,9 +48,9 @@ void test_call_twice(void) {
 
   animation.play(0);
   TEST_ASSERT_EQUAL(Animation::MODE_PLAY, animation.getMode());
-  animation.stop();
+  animation.stop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, animation.getMode());
-  animation.stop();
+  animation.stop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, animation.getMode());
 
   TEST_ASSERT_EQUAL(2, modeChangeCount);
@@ -60,7 +60,7 @@ void test_prevented(void) {
   Animation animation(FPS, FRAMES);
 
   TEST_ASSERT_EQUAL(Animation::MODE_DEFAULT, animation.getMode());
-  animation.stop();
+  animation.stop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_DEFAULT, animation.getMode());
 }
 
@@ -70,23 +70,23 @@ void test_allowed(void) {
 
   animation.play(0);
   TEST_ASSERT_EQUAL(Animation::MODE_PLAY, animation.getMode());
-  animation.stop();
+  animation.stop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, animation.getMode());
-  animation.run(0);
+  animation.run(10000);
   animation.loop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_LOOP, animation.getMode());
-  animation.stop();
+  animation.stop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, animation.getMode());
-  animation.run(0);
+  animation.run(10000);
   animation.play(0);
   animation.pause();
   TEST_ASSERT_EQUAL(Animation::MODE_PAUSE, animation.getMode());
-  animation.stop();
+  animation.stop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, animation.getMode());
-  animation.run(0);
+  animation.run(10000);
   animation.live(mock);
   TEST_ASSERT_EQUAL(Animation::MODE_LIVE, animation.getMode());
-  animation.stop();
+  animation.stop(0);
   TEST_ASSERT_EQUAL(Animation::MODE_STOP, animation.getMode());
 }
 
diff --git a/test/show/test_live/test_live.cpp b/test/show/test_live/test_live.cpp
@@ -50,7 +50,7 @@ void test_prevented(void) {
   TEST_ASSERT_EQUAL(Show::MODE_PAUSE, show.getMode());
   show.live(mock);
   TEST_ASSERT_EQUAL(Show::MODE_PAUSE, show.getMode());
-  show.stop();
+  show.stop(0);
   TEST_ASSERT_EQUAL(Show::MODE_STOP, show.getMode());
   show.live(mock);
   TEST_ASSERT_EQUAL(Show::MODE_STOP, show.getMode());
diff --git a/test/show/test_loop/test_loop.cpp b/test/show/test_loop/test_loop.cpp
@@ -73,11 +73,11 @@ void test_prevented(void) {
   TEST_ASSERT_EQUAL(Show::MODE_PLAY_RANDOM, show.getMode());
   show.loop(0);
   TEST_ASSERT_EQUAL(Show::MODE_PLAY_RANDOM, show.getMode());
-  show.stop();
+  show.stop(0);
   TEST_ASSERT_EQUAL(Show::MODE_STOP, show.getMode());
   show.loop(0);
   TEST_ASSERT_EQUAL(Show::MODE_STOP, show.getMode());
-  show.run(0);
+  show.run(10000);
   show.live(mock);
   TEST_ASSERT_EQUAL(Show::MODE_LIVE, show.getMode());
   show.loop(0);
diff --git a/test/show/test_mode_callback/test_mode_callback.cpp b/test/show/test_mode_callback/test_mode_callback.cpp
@@ -81,11 +81,11 @@ void test_all_modes(void) {
   TEST_ASSERT_EQUAL(Show::MODE_PAUSE, prevMode);
   TEST_ASSERT_EQUAL(Show::MODE_LOOP, newMode);
   TEST_ASSERT_EQUAL(0, show.getPlayIndex());
-  show.stop();
+  show.stop(0);
   TEST_ASSERT_EQUAL(Show::MODE_LOOP, prevMode);
   TEST_ASSERT_EQUAL(Show::MODE_STOP, newMode);
   TEST_ASSERT_EQUAL(0, show.getPlayIndex());
-  show.run(0);
+  show.run(10000);
   TEST_ASSERT_EQUAL(Show::MODE_STOP, prevMode);
   TEST_ASSERT_EQUAL(Show::MODE_DEFAULT, newMode);
   TEST_ASSERT_EQUAL(0, show.getPlayIndex());
diff --git a/test/show/test_pause/test_pause.cpp b/test/show/test_pause/test_pause.cpp
@@ -103,11 +103,11 @@ void test_prevented(void) {
   show.pause();
   TEST_ASSERT_EQUAL(Show::MODE_DEFAULT, show.getMode());
   show.play(0);
-  show.stop();
+  show.stop(0);
   TEST_ASSERT_EQUAL(Show::MODE_STOP, show.getMode());
   show.pause();
   TEST_ASSERT_EQUAL(Show::MODE_STOP, show.getMode());
-  show.run(0);
+  show.run(10000);
   show.live(mock);
   TEST_ASSERT_EQUAL(Show::MODE_LIVE, show.getMode());
   show.pause();
diff --git a/test/show/test_play/test_play.cpp b/test/show/test_play/test_play.cpp
@@ -82,11 +82,11 @@ void test_prevented(void) {
   TEST_ASSERT_EQUAL(Show::MODE_PLAY_RANDOM, show.getMode());
   show.play(0);
   TEST_ASSERT_EQUAL(Show::MODE_PLAY_RANDOM, show.getMode());
-  show.stop();
+  show.stop(0);
   TEST_ASSERT_EQUAL(Show::MODE_STOP, show.getMode());
   show.play(0);
   TEST_ASSERT_EQUAL(Show::MODE_STOP, show.getMode());
-  show.run(0);
+  show.run(10000);
   show.live(mock);
   TEST_ASSERT_EQUAL(Show::MODE_LIVE, show.getMode());
   show.play(0);
diff --git a/test/show/test_play_random/test_play_random.cpp b/test/show/test_play_random/test_play_random.cpp
@@ -73,11 +73,11 @@ void test_prevented(void) {
   TEST_ASSERT_EQUAL(Show::MODE_PLAY_SINGLE, show.getMode());
   show.playRandom(0);
   TEST_ASSERT_EQUAL(Show::MODE_PLAY_SINGLE, show.getMode());
-  show.stop();
+  show.stop(0);
   TEST_ASSERT_EQUAL(Show::MODE_STOP, show.getMode());
   show.playRandom(0);
   TEST_ASSERT_EQUAL(Show::MODE_STOP, show.getMode());
-  show.run(0);
+  show.run(10000);
   show.live(mock);
   TEST_ASSERT_EQUAL(Show::MODE_LIVE, show.getMode());
   show.playRandom(0);
diff --git a/test/show/test_play_single/test_play_single.cpp b/test/show/test_play_single/test_play_single.cpp
@@ -66,11 +66,11 @@ void test_prevented(void) {
   TEST_ASSERT_EQUAL(Show::MODE_PLAY_RANDOM, show.getMode());
   show.playSingle(0, 0);
   TEST_ASSERT_EQUAL(Show::MODE_PLAY_RANDOM, show.getMode());
-  show.stop();
+  show.stop(0);
   TEST_ASSERT_EQUAL(Show::MODE_STOP, show.getMode());
   show.playSingle(0, 0);
   TEST_ASSERT_EQUAL(Show::MODE_STOP, show.getMode());
-  show.run(0);
+  show.run(10000);
   show.live(mock);
   TEST_ASSERT_EQUAL(Show::MODE_LIVE, show.getMode());
   show.playSingle(0, 0);
diff --git a/test/show/test_stop/test_stop.cpp b/test/show/test_stop/test_stop.cpp
diff --git a/test/test_servo/test_servo.cpp b/test/test_servo/test_servo.cpp

Original file line number	Diff line number	Diff line change
`@@ -45,7 +45,7 @@ void Animation::run(unsigned long currentMicros) {`
`45`	`45`	`this->handlePlayMode(currentMicros);`
`46`	`46`	`break;`
`47`	`47`	`case MODE_STOP:`
`48`		`- this->handleStopMode();`
	`48`	`+ this->handleStopMode(currentMicros);`
`49`	`49`	`break;`
`50`	`50`	`case MODE_LIVE:`
`51`	`51`	`this->handleLiveMode();`
`@@ -88,9 +88,15 @@ void Animation::handlePlayMode(unsigned long currentMicros) {`
`88`	`88`	`}`
`89`	`89`	`}`
`90`	`90`
`91`		`-void Animation::handleStopMode() {`
	`91`	`+void Animation::handleStopMode(unsigned long currentMicros) {`
`92`	`92`	`bool allNeutral = true;`
`93`	`93`
	`94`	`+ if (currentMicros - this->lastMicros < 10000) {`
	`95`	`+ return;`
	`96`	`+ }`
	`97`	`+`
	`98`	`+ this->lastMicros = currentMicros;`
	`99`	`+`
`94`	`100`	`for (int i = 0; i < MAX_SERVO_COUNT; i++) {`
`95`	`101`	`Servo *servo = this->servos[i];`
`96`	`102`
`@@ -100,14 +106,11 @@ void Animation::handleStopMode() {`
`100`	`106`	`}`
`101`	`107`	`}`
`102`	`108`
`103`		`- if (allNeutral) {`
`104`		`- this->changeMode(MODE_DEFAULT);`
	`109`	`+ if (!allNeutral) {`
`105`	`110`	`return;`
`106`	`111`	`}`
`107`	`112`
`108`		`- if (this->stopStepDelay > 0) {`
`109`		`- delay(this->stopStepDelay);`
`110`		`- }`
	`113`	`+ this->changeMode(MODE_DEFAULT);`
`111`	`114`	`}`
`112`	`115`
`113`	`116`	`void Animation::handleLiveMode() {`
`@@ -158,13 +161,13 @@ void Animation::loop(unsigned long currentMicros) {`
`158`	`161`	`this->changeMode(MODE_LOOP);`
`159`	`162`	`}`
`160`	`163`
`161`		`-void Animation::stop(byte stepDelay) {`
	`164`	`+void Animation::stop(unsigned long currentMicros) {`
`162`	`165`	`if (this->modeIsIn(2, MODE_DEFAULT, MODE_STOP)) {`
`163`	`166`	`return;`
`164`	`167`	`}`
`165`	`168`
`166`		`- this->stopStepDelay = stepDelay;`
`167`	`169`	`this->frame = 0;`
	`170`	`+ this->lastMicros = currentMicros;`
`168`	`171`	`this->changeMode(MODE_STOP);`
`169`	`172`	`}`
`170`	`173`
Original file line number	Diff line number	Diff line change
`@@ -40,6 +40,7 @@ void Servo::moveByFrame(int frame) {`
`40`	`40`	`}`
`41`	`41`
`42`	`42`	`void Servo::moveTowardsNeutral(bool inSteps) {`
	`43`	`+ int step = round(this->threshold / 10);`
`43`	`44`	`int newPosition = this->currentPosition;`
`44`	`45`
`45`	`46`	`if (inSteps == false) {`
`@@ -48,9 +49,13 @@ void Servo::moveTowardsNeutral(bool inSteps) {`
`48`	`49`	`}`
`49`	`50`
`50`	`51`	`if (this->currentPosition > this->neutralPosition) {`
`51`		`- newPosition--;`
	`52`	`+ newPosition -= step;`
`52`	`53`	`} else if (this->currentPosition < this->neutralPosition) {`
`53`		`- newPosition++;`
	`54`	`+ newPosition += step;`
	`55`	`+ }`
	`56`	`+`
	`57`	`+ if (abs(this->neutralPosition - newPosition) < step) {`
	`58`	`+ newPosition = this->neutralPosition;`
`54`	`59`	`}`
`55`	`60`
`56`	`61`	`this->move(newPosition);`
Original file line number	Diff line number	Diff line change
`@@ -104,12 +104,12 @@ void Show::pause() {`
`104`	`104`	`this->changeMode(MODE_PAUSE);`
`105`	`105`	`}`
`106`	`106`
`107`		`-void Show::stop(byte stepDelay) {`
	`107`	`+void Show::stop(unsigned long currentMicros) {`
`108`	`108`	`if (!this->animation \|\| this->modeIsIn(2, MODE_DEFAULT, MODE_STOP)) {`
`109`	`109`	`return;`
`110`	`110`	`}`
`111`	`111`
`112`		`- this->animation->stop(stepDelay);`
	`112`	`+ this->animation->stop(currentMicros);`
`113`	`113`	`this->changeMode(MODE_STOP);`
`114`	`114`	`}`
`115`	`115`