ESPHome  2024.3.1
servo.cpp
Go to the documentation of this file.
1 #include "servo.h"
2 #include "esphome/core/log.h"
3 #include "esphome/core/hal.h"
4 #include <cinttypes>
5 
6 namespace esphome {
7 namespace servo {
8 
9 static const char *const TAG = "servo";
10 
11 uint32_t global_servo_id = 1911044085ULL; // NOLINT(cppcoreguidelines-avoid-non-const-global-variables)
12 
14  ESP_LOGCONFIG(TAG, "Servo:");
15  ESP_LOGCONFIG(TAG, " Idle Level: %.1f%%", this->idle_level_ * 100.0f);
16  ESP_LOGCONFIG(TAG, " Min Level: %.1f%%", this->min_level_ * 100.0f);
17  ESP_LOGCONFIG(TAG, " Max Level: %.1f%%", this->max_level_ * 100.0f);
18  ESP_LOGCONFIG(TAG, " auto detach time: %" PRIu32 " ms", this->auto_detach_time_);
19  ESP_LOGCONFIG(TAG, " run duration: %" PRIu32 " ms", this->transition_length_);
20 }
21 
22 void Servo::setup() {
23  float v;
24  if (this->restore_) {
26  global_servo_id++;
27  if (this->rtc_.load(&v)) {
28  this->target_value_ = v;
29  this->internal_write(v);
30  this->state_ = STATE_ATTACHED;
31  this->start_millis_ = millis();
32  return;
33  }
34  }
35  this->detach();
36 }
37 
38 void Servo::loop() {
39  // check if auto_detach_time_ is set and servo reached target
40  if (this->auto_detach_time_ && this->state_ == STATE_TARGET_REACHED) {
41  if (millis() - this->start_millis_ > this->auto_detach_time_) {
42  this->detach();
43  this->start_millis_ = 0;
44  ESP_LOGD(TAG, "Servo detached on auto_detach_time");
45  }
46  }
47  if (this->target_value_ != this->current_value_ && this->state_ == STATE_ATTACHED) {
48  if (this->transition_length_) {
49  float new_value;
50  float travel_diff = this->target_value_ - this->source_value_;
51  uint32_t target_runtime = abs((int) ((travel_diff) * this->transition_length_ * 1.0f / 2.0f));
52  uint32_t current_runtime = millis() - this->start_millis_;
53  float percentage_run = current_runtime * 1.0f / target_runtime * 1.0f;
54  if (percentage_run > 1.0f) {
55  percentage_run = 1.0f;
56  }
57  new_value = this->target_value_ - (1.0f - percentage_run) * (this->target_value_ - this->source_value_);
58  this->internal_write(new_value);
59  } else {
60  this->internal_write(this->target_value_);
61  }
62  }
63  if (this->target_value_ == this->current_value_ && this->state_ == STATE_ATTACHED) {
65  this->start_millis_ = millis(); // set current stamp for potential auto_detach_time_ check
66  ESP_LOGD(TAG, "Servo reached target");
67  }
68 }
69 
70 void Servo::write(float value) {
71  value = clamp(value, -1.0f, 1.0f);
72  if ((this->state_ == STATE_DETACHED) && (this->target_value_ == value)) {
73  this->internal_write(value);
74  } else {
75  this->save_level_(value);
76  }
77  this->target_value_ = value;
78  this->source_value_ = this->current_value_;
79  this->state_ = STATE_ATTACHED;
80  this->start_millis_ = millis();
81  ESP_LOGD(TAG, "Servo new target: %f", value);
82 }
83 
84 void Servo::internal_write(float value) {
85  value = clamp(value, -1.0f, 1.0f);
86  float level;
87  if (value < 0.0) {
88  level = lerp(-value, this->idle_level_, this->min_level_);
89  } else {
90  level = lerp(value, this->idle_level_, this->max_level_);
91  }
92  this->output_->set_level(level);
93  this->current_value_ = value;
94 }
95 
96 void Servo::detach() {
97  this->state_ = STATE_DETACHED;
98  this->output_->set_level(0.0f);
99 }
100 
101 void Servo::save_level_(float v) {
102  if (this->restore_)
103  this->rtc_.save(&v);
104 }
105 
106 } // namespace servo
107 } // namespace esphome
uint8_t state_
Definition: servo.h:44
uint32_t transition_length_
Definition: servo.h:42
void save_level_(float v)
Definition: servo.cpp:101
void write(float value)
Definition: servo.cpp:70
float target_value_
Definition: servo.h:45
float lerp(float completion, float start, float end)
Linearly interpolate between start and end by completion (between 0 and 1).
Definition: helpers.cpp:95
uint32_t start_millis_
Definition: servo.h:48
constexpr const T & clamp(const T &v, const T &lo, const T &hi, Compare comp)
Definition: helpers.h:92
uint32_t IRAM_ATTR HOT millis()
Definition: core.cpp:25
bool save(const T *src)
Definition: preferences.h:21
void set_level(float state)
Set the level of this float output, this is called from the front-end.
float current_value_
Definition: servo.h:47
uint32_t auto_detach_time_
Definition: servo.h:41
ESPPreferenceObject rtc_
Definition: servo.h:43
void loop() override
Definition: servo.cpp:38
void internal_write(float value)
Definition: servo.cpp:84
ESPPreferences * global_preferences
output::FloatOutput * output_
Definition: servo.h:36
uint32_t global_servo_id
Definition: servo.cpp:11
float source_value_
Definition: servo.h:46
void setup() override
Definition: servo.cpp:22
void dump_config() override
Definition: servo.cpp:13
virtual ESPPreferenceObject make_preference(size_t length, uint32_t type, bool in_flash)=0
This is a workaround until we can figure out a way to get the tflite-micro idf component code availab...
Definition: a01nyub.cpp:7