diff --git a/esphome/components/servo/__init__.py b/esphome/components/servo/__init__.py index 9b06159c13..76690dbcf3 100644 --- a/esphome/components/servo/__init__.py +++ b/esphome/components/servo/__init__.py @@ -4,13 +4,14 @@ from esphome import automation from esphome.automation import maybe_simple_id from esphome.components.output import FloatOutput from esphome.const import CONF_ID, CONF_IDLE_LEVEL, CONF_MAX_LEVEL, CONF_MIN_LEVEL, CONF_OUTPUT, \ - CONF_LEVEL, CONF_RESTORE + CONF_LEVEL, CONF_RESTORE, CONF_TRANSITION_LENGTH servo_ns = cg.esphome_ns.namespace('servo') Servo = servo_ns.class_('Servo', cg.Component) ServoWriteAction = servo_ns.class_('ServoWriteAction', automation.Action) ServoDetachAction = servo_ns.class_('ServoDetachAction', automation.Action) +CONF_AUTO_DETACH_TIME = 'auto_detach_time' MULTI_CONF = True CONFIG_SCHEMA = cv.Schema({ cv.Required(CONF_ID): cv.declare_id(Servo), @@ -19,6 +20,8 @@ CONFIG_SCHEMA = cv.Schema({ cv.Optional(CONF_IDLE_LEVEL, default='7.5%'): cv.percentage, cv.Optional(CONF_MAX_LEVEL, default='12%'): cv.percentage, cv.Optional(CONF_RESTORE, default=False): cv.boolean, + cv.Optional(CONF_AUTO_DETACH_TIME, default='0s'): cv.positive_time_period_milliseconds, + cv.Optional(CONF_TRANSITION_LENGTH, default='0s'): cv.positive_time_period_milliseconds }).extend(cv.COMPONENT_SCHEMA) @@ -32,6 +35,8 @@ def to_code(config): cg.add(var.set_idle_level(config[CONF_IDLE_LEVEL])) cg.add(var.set_max_level(config[CONF_MAX_LEVEL])) cg.add(var.set_restore(config[CONF_RESTORE])) + cg.add(var.set_auto_detach_time(config[CONF_AUTO_DETACH_TIME])) + cg.add(var.set_transition_length(config[CONF_TRANSITION_LENGTH])) @automation.register_action('servo.write', ServoWriteAction, cv.Schema({ diff --git a/esphome/components/servo/servo.cpp b/esphome/components/servo/servo.cpp index 2fade280ee..5d2a9ee236 100644 --- a/esphome/components/servo/servo.cpp +++ b/esphome/components/servo/servo.cpp @@ -13,6 +13,64 @@ void Servo::dump_config() { ESP_LOGCONFIG(TAG, " Idle Level: %.1f%%", this->idle_level_ * 100.0f); ESP_LOGCONFIG(TAG, " Min Level: %.1f%%", this->min_level_ * 100.0f); ESP_LOGCONFIG(TAG, " Max Level: %.1f%%", this->max_level_ * 100.0f); + ESP_LOGCONFIG(TAG, " auto detach time: %d ms", this->auto_detach_time_); + ESP_LOGCONFIG(TAG, " run duration: %d ms", this->transition_length_); +} + +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"); + } + } + if (this->target_value_ != this->current_value_ && this->state_ == STATE_ATTACHED) { + if (this->transition_length_) { + float new_value; + float travel_diff = this->target_value_ - this->source_value_; + uint32_t target_runtime = target_runtime = abs((int) ((travel_diff) * this->transition_length_ * 1.0f / 2.0f)); + uint32_t current_runtime = millis() - this->start_millis_; + float percentage_run = current_runtime * 1.0f / target_runtime * 1.0f; + if (percentage_run > 1.0f) { + percentage_run = 1.0f; + } + new_value = this->target_value_ - (1.0f - percentage_run) * (this->target_value_ - this->source_value_); + this->internal_write(new_value); + } else { + this->internal_write(this->target_value_); + } + } + if (this->target_value_ == this->current_value_ && this->state_ == STATE_ATTACHED) { + this->state_ = STATE_TARGET_REACHED; + this->start_millis_ = millis(); // set current stamp for potential auto_detach_time_ check + ESP_LOGD(TAG, "Servo reached target"); + } +} + +void Servo::write(float value) { + value = clamp(value, -1.0f, 1.0f); + this->target_value_ = value; + this->source_value_ = this->current_value_; + this->state_ = STATE_ATTACHED; + this->start_millis_ = millis(); + ESP_LOGD(TAG, "Servo new target: %f", value); +} + +void Servo::internal_write(float value) { + value = clamp(value, -1.0f, 1.0f); + float level; + if (value < 0.0) + level = lerp(-value, this->idle_level_, this->min_level_); + else + 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; } } // namespace servo diff --git a/esphome/components/servo/servo.h b/esphome/components/servo/servo.h index b864efc877..937706d44d 100644 --- a/esphome/components/servo/servo.h +++ b/esphome/components/servo/servo.h @@ -14,18 +14,9 @@ extern uint32_t global_servo_id; class Servo : public Component { public: void set_output(output::FloatOutput *output) { output_ = output; } - void write(float value) { - value = clamp(value, -1.0f, 1.0f); - - float level; - if (value < 0.0) - level = lerp(-value, this->idle_level_, this->min_level_); - else - level = lerp(value, this->idle_level_, this->max_level_); - - this->output_->set_level(level); - this->save_level_(level); - } + 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); @@ -48,6 +39,8 @@ class Servo : public Component { void set_idle_level(float idle_level) { idle_level_ = idle_level; } void set_max_level(float max_level) { max_level_ = max_level; } void set_restore(bool restore) { restore_ = restore; } + 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; } protected: void save_level_(float v) { this->rtc_.save(&v); } @@ -57,7 +50,19 @@ class Servo : public Component { float idle_level_ = 0.0750f; float max_level_ = 0.1200f; bool restore_{false}; + uint32_t auto_detach_time_ = 0; + uint32_t transition_length_ = 0; ESPPreferenceObject rtc_; + uint8_t state_; + float target_value_ = 0; + float source_value_ = 0; + float current_value_ = 0; + uint32_t start_millis_ = 0; + enum State { + STATE_ATTACHED = 0, + STATE_DETACHED = 1, + STATE_TARGET_REACHED = 2, + }; }; template class ServoWriteAction : public Action {