From 92fbc61c46d4229e00bb5fbc42b0f22b55126330 Mon Sep 17 00:00:00 2001 From: Samuel Sieb Date: Wed, 13 Mar 2024 22:08:57 -0700 Subject: [PATCH] fix servo restore (#6370) --- esphome/components/servo/servo.cpp | 35 +++++++++++++++++++++++++----- esphome/components/servo/servo.h | 22 +++++-------------- 2 files changed, 35 insertions(+), 22 deletions(-) diff --git a/esphome/components/servo/servo.cpp b/esphome/components/servo/servo.cpp index 666c017dea..18e8c8087e 100644 --- a/esphome/components/servo/servo.cpp +++ b/esphome/components/servo/servo.cpp @@ -19,13 +19,28 @@ void Servo::dump_config() { ESP_LOGCONFIG(TAG, " run duration: %" PRIu32 " ms", this->transition_length_); } +void Servo::setup() { + float v; + if (this->restore_) { + this->rtc_ = global_preferences->make_preference(global_servo_id); + global_servo_id++; + if (this->rtc_.load(&v)) { + this->target_value_ = v; + this->internal_write(v); + this->state_ = STATE_ATTACHED; + this->start_millis_ = millis(); + return; + } + } + this->detach(); +} + void Servo::loop() { // check if auto_detach_time_ is set and servo reached target if (this->auto_detach_time_ && this->state_ == STATE_TARGET_REACHED) { if (millis() - this->start_millis_ > this->auto_detach_time_) { this->detach(); this->start_millis_ = 0; - this->state_ = STATE_DETACHED; ESP_LOGD(TAG, "Servo detached on auto_detach_time"); } } @@ -54,8 +69,11 @@ void Servo::loop() { void Servo::write(float value) { value = clamp(value, -1.0f, 1.0f); - if (this->target_value_ == value) + if ((this->state_ == STATE_DETACHED) && (this->target_value_ == value)) { this->internal_write(value); + } else { + this->save_level_(value); + } this->target_value_ = value; this->source_value_ = this->current_value_; this->state_ = STATE_ATTACHED; @@ -72,11 +90,18 @@ void Servo::internal_write(float value) { level = lerp(value, this->idle_level_, this->max_level_); } this->output_->set_level(level); - if (this->target_value_ == this->current_value_) { - this->save_level_(level); - } this->current_value_ = value; } +void Servo::detach() { + this->state_ = STATE_DETACHED; + this->output_->set_level(0.0f); +} + +void Servo::save_level_(float v) { + if (this->restore_) + this->rtc_.save(&v); +} + } // namespace servo } // namespace esphome diff --git a/esphome/components/servo/servo.h b/esphome/components/servo/servo.h index e2e3823158..13a7472ae5 100644 --- a/esphome/components/servo/servo.h +++ b/esphome/components/servo/servo.h @@ -17,22 +17,8 @@ class Servo : public Component { void loop() override; void write(float value); void internal_write(float value); - void detach() { - this->output_->set_level(0.0f); - this->save_level_(0.0f); - } - void setup() override { - float v; - if (this->restore_) { - this->rtc_ = global_preferences->make_preference(global_servo_id); - global_servo_id++; - if (this->rtc_.load(&v)) { - this->output_->set_level(v); - return; - } - } - this->detach(); - } + void detach(); + void setup() override; void dump_config() override; float get_setup_priority() const override { return setup_priority::DATA; } void set_min_level(float min_level) { min_level_ = min_level; } @@ -42,8 +28,10 @@ class Servo : public Component { void set_auto_detach_time(uint32_t auto_detach_time) { auto_detach_time_ = auto_detach_time; } void set_transition_length(uint32_t transition_length) { transition_length_ = transition_length; } + bool has_reached_target() { return this->current_value_ == this->target_value_; } + protected: - void save_level_(float v) { this->rtc_.save(&v); } + void save_level_(float v); output::FloatOutput *output_; float min_level_ = 0.0300f;