From 92fbc61c46d4229e00bb5fbc42b0f22b55126330 Mon Sep 17 00:00:00 2001
From: Samuel Sieb <samuel-github@sieb.net>
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<float>(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<float>(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;