ESPHome  2022.8.0
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 
5 namespace esphome {
6 namespace servo {
7 
8 static const char *const TAG = "servo";
9 
10 uint32_t global_servo_id = 1911044085ULL; // NOLINT(cppcoreguidelines-avoid-non-const-global-variables)
11 
13  ESP_LOGCONFIG(TAG, "Servo:");
14  ESP_LOGCONFIG(TAG, " Idle Level: %.1f%%", this->idle_level_ * 100.0f);
15  ESP_LOGCONFIG(TAG, " Min Level: %.1f%%", this->min_level_ * 100.0f);
16  ESP_LOGCONFIG(TAG, " Max Level: %.1f%%", this->max_level_ * 100.0f);
17  ESP_LOGCONFIG(TAG, " auto detach time: %d ms", this->auto_detach_time_);
18  ESP_LOGCONFIG(TAG, " run duration: %d ms", this->transition_length_);
19 }
20 
21 void Servo::loop() {
22  // check if auto_detach_time_ is set and servo reached target
23  if (this->auto_detach_time_ && this->state_ == STATE_TARGET_REACHED) {
24  if (millis() - this->start_millis_ > this->auto_detach_time_) {
25  this->detach();
26  this->start_millis_ = 0;
27  this->state_ = STATE_DETACHED;
28  ESP_LOGD(TAG, "Servo detached on auto_detach_time");
29  }
30  }
31  if (this->target_value_ != this->current_value_ && this->state_ == STATE_ATTACHED) {
32  if (this->transition_length_) {
33  float new_value;
34  float travel_diff = this->target_value_ - this->source_value_;
35  uint32_t target_runtime = abs((int) ((travel_diff) * this->transition_length_ * 1.0f / 2.0f));
36  uint32_t current_runtime = millis() - this->start_millis_;
37  float percentage_run = current_runtime * 1.0f / target_runtime * 1.0f;
38  if (percentage_run > 1.0f) {
39  percentage_run = 1.0f;
40  }
41  new_value = this->target_value_ - (1.0f - percentage_run) * (this->target_value_ - this->source_value_);
42  this->internal_write(new_value);
43  } else {
44  this->internal_write(this->target_value_);
45  }
46  }
47  if (this->target_value_ == this->current_value_ && this->state_ == STATE_ATTACHED) {
49  this->start_millis_ = millis(); // set current stamp for potential auto_detach_time_ check
50  ESP_LOGD(TAG, "Servo reached target");
51  }
52 }
53 
54 void Servo::write(float value) {
55  value = clamp(value, -1.0f, 1.0f);
56  if (this->target_value_ == value)
57  this->internal_write(value);
58  this->target_value_ = value;
59  this->source_value_ = this->current_value_;
60  this->state_ = STATE_ATTACHED;
61  this->start_millis_ = millis();
62  ESP_LOGD(TAG, "Servo new target: %f", value);
63 }
64 
65 void Servo::internal_write(float value) {
66  value = clamp(value, -1.0f, 1.0f);
67  float level;
68  if (value < 0.0) {
69  level = lerp(-value, this->idle_level_, this->min_level_);
70  } else {
71  level = lerp(value, this->idle_level_, this->max_level_);
72  }
73  this->output_->set_level(level);
74  if (this->target_value_ == this->current_value_) {
75  this->save_level_(level);
76  }
77  this->current_value_ = value;
78 }
79 
80 } // namespace servo
81 } // namespace esphome
uint8_t state_
Definition: servo.h:56
uint32_t transition_length_
Definition: servo.h:54
void save_level_(float v)
Definition: servo.h:46
void write(float value)
Definition: servo.cpp:54
float target_value_
Definition: servo.h:57
float lerp(float completion, float start, float end)
Linearly interpolate between start and end by completion (between 0 and 1).
Definition: helpers.cpp:49
uint32_t start_millis_
Definition: servo.h:60
constexpr const T & clamp(const T &v, const T &lo, const T &hi, Compare comp)
Definition: helpers.h:84
uint32_t IRAM_ATTR HOT millis()
Definition: core.cpp:26
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:59
uint32_t auto_detach_time_
Definition: servo.h:53
void loop() override
Definition: servo.cpp:21
void internal_write(float value)
Definition: servo.cpp:65
output::FloatOutput * output_
Definition: servo.h:48
uint32_t global_servo_id
Definition: servo.cpp:10
float source_value_
Definition: servo.h:58
void dump_config() override
Definition: servo.cpp:12
Definition: a4988.cpp:4