diff --git a/src/ShadeMotion.cpp b/src/ShadeMotion.cpp index b1b2e9c..ff265ec 100644 --- a/src/ShadeMotion.cpp +++ b/src/ShadeMotion.cpp @@ -30,6 +30,12 @@ int8_t displayPosition(float pos, bool flip) { return static_cast(flip && pos >= 0.00f ? floor(100.0f - pos) : floor(pos)); } float limitTarget(int8_t dir) { return dir > 0 ? 100.0f : 0.0f; } +bool stepPosition(float pos, int8_t dir, uint32_t travelMs, uint16_t stepSize, uint8_t frameStepSize, float &out) { + if(travelMs == 0 || stepSize == 0) return false; + const float step = 100.0f / (static_cast(travelMs) / static_cast(stepSize * frameStepSize)); + out = dir > 0 ? std::min(100.0f, pos + step) : std::max(0.0f, pos - step); + return true; +} void TxHold::mark(uint32_t jobSeq, bool moving, uint32_t now) { if(jobSeq == 0 || this->seq != 0 || moving) return; this->seq = jobSeq; diff --git a/src/ShadeMotion.h b/src/ShadeMotion.h index dff2c14..f738760 100644 --- a/src/ShadeMotion.h +++ b/src/ShadeMotion.h @@ -15,6 +15,11 @@ float travelPosition(float startPos, int8_t dir, uint32_t elapsedMs, int32_t tra int8_t displayPosition(float pos, bool flip); // Where a shade (or its tilt) moving in dir ends up when nothing stops it. float limitTarget(int8_t dir); +// Where a step command moves a shade (or its tilt) from pos, in out. dir > 0 +// steps toward 100 and dir < 0 toward 0. A step is stepSize * frameStepSize +// ms of the travelMs a full travel takes in that direction. False when +// travelMs or stepSize is 0, so the step is ignored. +bool stepPosition(float pos, int8_t dir, uint32_t travelMs, uint16_t stepSize, uint8_t frameStepSize, float &out); // What a held shade does on this pass of checkMovement(). enum class TxHoldCheck { diff --git a/src/Somfy.cpp b/src/Somfy.cpp index 21a049a..0f21f78 100644 --- a/src/Somfy.cpp +++ b/src/Somfy.cpp @@ -1745,6 +1745,7 @@ void SomfyShade::processFrame(somfy_frame_t &frame, bool internal) { const uint64_t curTime = millis(); this->lastFrame.copy(frame); int8_t dir = 0; + float step; // Where a step command takes the shade. this->moveStart = this->tiltStart = curTime; this->startPos = this->currentPos; this->startTiltPos = this->currentTiltPos; @@ -2004,23 +2005,23 @@ void SomfyShade::processFrame(somfy_frame_t &frame, bool internal) { else if(this->currentTiltPos > 0.0f) { // Set the tilt position. This should stop the lift movement. this->p_target(this->currentPos); - if(this->tiltTime == 0) return; // Avoid divide by 0. - this->p_tiltTarget(max(0.0f, this->currentTiltPos - (100.0f/(static_cast(this->tiltTime/static_cast(this->stepSize * this->lastFrame.stepSize)))))); + if(!stepPosition(this->currentTiltPos, -1, this->tiltTime, this->stepSize, this->lastFrame.stepSize, step)) return; + this->p_tiltTarget(step); } else { // We only have the lift to move. - if(this->upTime == 0) return; // Avoid divide by 0. + if(!stepPosition(this->currentPos, -1, this->upTime, this->stepSize, this->lastFrame.stepSize, step)) return; this->p_tiltTarget(this->currentTiltPos); - this->p_target(max(0.0f, this->currentPos - (100.0f/(static_cast(this->upTime/static_cast(this->stepSize * this->lastFrame.stepSize)))))); + this->p_target(step); } } else if(this->tiltType == tilt_types::tiltonly) { - if(this->tiltTime == 0 || this->stepSize == 0) return; - this->p_tiltTarget(max(0.0f, this->currentTiltPos - (100.0f/(static_cast(this->tiltTime/static_cast(this->stepSize * this->lastFrame.stepSize)))))); + if(!stepPosition(this->currentTiltPos, -1, this->tiltTime, this->stepSize, this->lastFrame.stepSize, step)) return; + this->p_tiltTarget(step); } else if(this->currentPos > 0.0f) { - if(this->upTime == 0 || this->stepSize == 0) return; - this->p_target(max(0.0f, this->currentPos - (100.0f/(static_cast(this->upTime/static_cast(this->stepSize * this->lastFrame.stepSize)))))); + if(!stepPosition(this->currentPos, -1, this->upTime, this->stepSize, this->lastFrame.stepSize, step)) return; + this->p_target(step); } this->emitCommand(cmd, internal ? "internal" : "remote", frame.remoteAddress); break; @@ -2046,23 +2047,23 @@ void SomfyShade::processFrame(somfy_frame_t &frame, bool internal) { else if(this->currentTiltPos < 100.0f) { // Set the tilt position. This should stop the lift movement. this->p_target(this->currentPos); - if(this->tiltTime == 0) return; // Avoid divide by 0. - this->p_tiltTarget(min(100.0f, this->currentTiltPos + (100.0f/(static_cast(this->tiltTime/static_cast(this->stepSize * this->lastFrame.stepSize)))))); + if(!stepPosition(this->currentTiltPos, 1, this->tiltTime, this->stepSize, this->lastFrame.stepSize, step)) return; + this->p_tiltTarget(step); } else { // We only have the lift to move. this->p_tiltTarget(this->currentTiltPos); - if(this->downTime == 0) return; // Avoid divide by 0. - this->p_target(min(100.0f, this->currentPos + (100.0f/(static_cast(this->downTime/static_cast(this->stepSize* this->lastFrame.stepSize)))))); + if(!stepPosition(this->currentPos, 1, this->downTime, this->stepSize, this->lastFrame.stepSize, step)) return; + this->p_target(step); } } else if(this->tiltType == tilt_types::tiltonly) { - if(this->tiltTime == 0 || this->stepSize == 0) return; - this->p_tiltTarget(min(100.0f, this->currentTiltPos + (100.0f/(static_cast(this->tiltTime/static_cast(this->stepSize * this->lastFrame.stepSize)))))); + if(!stepPosition(this->currentTiltPos, 1, this->tiltTime, this->stepSize, this->lastFrame.stepSize, step)) return; + this->p_tiltTarget(step); } else if(this->currentPos < 100.0f) { - if(this->downTime == 0 || this->stepSize == 0) return; - this->p_target(min(100.0f, this->currentPos + (100.0f/(static_cast(this->downTime/static_cast(this->stepSize * this->lastFrame.stepSize)))))); + if(!stepPosition(this->currentPos, 1, this->downTime, this->stepSize, this->lastFrame.stepSize, step)) return; + this->p_target(step); } this->emitCommand(cmd, internal ? "internal" : "remote", frame.remoteAddress); break; diff --git a/test/test_motion/test_motion.cpp b/test/test_motion/test_motion.cpp index 92f4822..0e5a9c1 100644 --- a/test/test_motion/test_motion.cpp +++ b/test/test_motion/test_motion.cpp @@ -78,6 +78,38 @@ static void test_moves_steadily() { } } +/********************************************************************* + * stepPosition + ********************************************************************/ +static void test_step_down_and_up() { + // A step of 100ms x 2 is 200ms of a 10s travel, 2%. + float out = -1; + TEST_ASSERT_TRUE(stepPosition(40, DOWN, 10000, 100, 2, out)); + ASSERT_POS(42.0f, out); + TEST_ASSERT_TRUE(stepPosition(40, UP, 10000, 100, 2, out)); + ASSERT_POS(38.0f, out); +} +static void test_step_stops_at_the_ends() { + float out = -1; + TEST_ASSERT_TRUE(stepPosition(99.5f, DOWN, 1000, 100, 1, out)); + ASSERT_POS(100.0f, out); + TEST_ASSERT_TRUE(stepPosition(0.5f, UP, 1000, 100, 1, out)); + ASSERT_POS(0.0f, out); +} +static void test_step_is_ignored_without_a_travel_time_or_step_size() { + // A shade with no time configured for the direction it steps in. + float out = -1; + TEST_ASSERT_FALSE(stepPosition(40, UP, 0, 100, 1, out)); + TEST_ASSERT_FALSE(stepPosition(40, DOWN, 0, 100, 1, out)); + TEST_ASSERT_FALSE(stepPosition(40, DOWN, 10000, 0, 1, out)); + ASSERT_POS(-1.0f, out); +} +static void test_step_of_no_frame_steps_stays_put() { + float out = -1; + TEST_ASSERT_TRUE(stepPosition(40, DOWN, 10000, 100, 0, out)); + ASSERT_POS(40.0f, out); +} + /********************************************************************* * displayPosition ********************************************************************/ @@ -191,6 +223,10 @@ int main(int argc, char **argv) { RUN_TEST(test_up_and_down_use_their_own_times); RUN_TEST(test_zero_travel_time_is_instant); RUN_TEST(test_moves_steadily); + RUN_TEST(test_step_down_and_up); + RUN_TEST(test_step_stops_at_the_ends); + RUN_TEST(test_step_is_ignored_without_a_travel_time_or_step_size); + RUN_TEST(test_step_of_no_frame_steps_stays_put); RUN_TEST(test_displayPosition); RUN_TEST(test_flipCommand_swaps_directions); RUN_TEST(test_flipCommand_keeps_the_rest);