mirror of
https://github.com/esphome/esphome.git
synced 2025-09-13 16:52:18 +01:00
111 lines
3.4 KiB
C++
111 lines
3.4 KiB
C++
#include "servo.h"
|
|
#include "esphome/core/log.h"
|
|
#include "esphome/core/hal.h"
|
|
#include <cinttypes>
|
|
|
|
namespace esphome {
|
|
namespace servo {
|
|
|
|
static const char *const TAG = "servo";
|
|
|
|
uint32_t global_servo_id = 1911044085ULL; // NOLINT(cppcoreguidelines-avoid-non-const-global-variables)
|
|
|
|
void Servo::dump_config() {
|
|
ESP_LOGCONFIG(TAG,
|
|
"Servo:\n"
|
|
" Idle Level: %.1f%%\n"
|
|
" Min Level: %.1f%%\n"
|
|
" Max Level: %.1f%%\n"
|
|
" Auto-detach time: %" PRIu32 " ms\n"
|
|
" Run duration: %" PRIu32 " ms",
|
|
this->idle_level_ * 100.0f, this->min_level_ * 100.0f, this->max_level_ * 100.0f,
|
|
this->auto_detach_time_, 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;
|
|
ESP_LOGD(TAG, "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 = 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, "Reached target");
|
|
}
|
|
}
|
|
|
|
void Servo::write(float value) {
|
|
value = clamp(value, -1.0f, 1.0f);
|
|
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;
|
|
this->start_millis_ = millis();
|
|
ESP_LOGD(TAG, "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);
|
|
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
|