ESPHome  2022.6.3
stepper.cpp
Go to the documentation of this file.
1 #include "stepper.h"
2 #include "esphome/core/log.h"
3 #include "esphome/core/hal.h"
4 
5 namespace esphome {
6 namespace stepper {
7 
8 static const char *const TAG = "stepper";
9 
10 void Stepper::calculate_speed_(uint32_t now) {
11  // delta t since last calculation in seconds
12  float dt = (now - this->last_calculation_) * 1e-6f;
13  this->last_calculation_ = now;
14  if (this->has_reached_target()) {
15  this->current_speed_ = 0.0f;
16  return;
17  }
18 
19  int32_t num_steps = abs(int32_t(this->target_position) - int32_t(this->current_position));
20  // (v_0)^2 / 2*a
21  float v_squared = this->current_speed_ * this->current_speed_;
22  auto steps_to_decelerate = static_cast<int32_t>(v_squared / (2 * this->deceleration_));
23  if (num_steps <= steps_to_decelerate) {
24  // need to start decelerating
25  this->current_speed_ -= this->deceleration_ * dt;
26  } else {
27  // we can still accelerate
28  this->current_speed_ += this->acceleration_ * dt;
29  }
30  this->current_speed_ = clamp(this->current_speed_, 0.0f, this->max_speed_);
31 }
33  uint32_t now = micros();
34  this->calculate_speed_(now);
35  if (this->current_speed_ == 0.0f)
36  return 0;
37 
38  // assumes this method is called in a constant interval
39  uint32_t dt = now - this->last_step_;
40  if (dt >= (1 / this->current_speed_) * 1e6f) {
41  int32_t mag = this->target_position > this->current_position ? 1 : -1;
42  this->last_step_ = now;
43  this->current_position += mag;
44  return mag;
45  }
46 
47  return 0;
48 }
49 
50 } // namespace stepper
51 } // namespace esphome
uint32_t last_calculation_
Definition: stepper.h:36
void calculate_speed_(uint32_t now)
Definition: stepper.cpp:10
uint32_t IRAM_ATTR HOT micros()
Definition: core.cpp:28
constexpr const T & clamp(const T &v, const T &lo, const T &hi, Compare comp)
Definition: helpers.h:84
Definition: a4988.cpp:4