mirror of
https://github.com/esphome/esphome.git
synced 2025-02-24 13:58:14 +00:00
51 lines
1.5 KiB
C++
51 lines
1.5 KiB
C++
#include "stepper.h"
|
|
#include "esphome/core/log.h"
|
|
|
|
namespace esphome {
|
|
namespace stepper {
|
|
|
|
static const char *const TAG = "stepper";
|
|
|
|
void Stepper::calculate_speed_(uint32_t now) {
|
|
// delta t since last calculation in seconds
|
|
float dt = (now - this->last_calculation_) * 1e-6f;
|
|
this->last_calculation_ = now;
|
|
if (this->has_reached_target()) {
|
|
this->current_speed_ = 0.0f;
|
|
return;
|
|
}
|
|
|
|
int32_t num_steps = abs(int32_t(this->target_position) - int32_t(this->current_position));
|
|
// (v_0)^2 / 2*a
|
|
float v_squared = this->current_speed_ * this->current_speed_;
|
|
auto steps_to_decelerate = static_cast<int32_t>(v_squared / (2 * this->deceleration_));
|
|
if (num_steps <= steps_to_decelerate) {
|
|
// need to start decelerating
|
|
this->current_speed_ -= this->deceleration_ * dt;
|
|
} else {
|
|
// we can still accelerate
|
|
this->current_speed_ += this->acceleration_ * dt;
|
|
}
|
|
this->current_speed_ = clamp(this->current_speed_, 0.0f, this->max_speed_);
|
|
}
|
|
int32_t Stepper::should_step_() {
|
|
uint32_t now = micros();
|
|
this->calculate_speed_(now);
|
|
if (this->current_speed_ == 0.0f)
|
|
return 0;
|
|
|
|
// assumes this method is called in a constant interval
|
|
uint32_t dt = now - this->last_step_;
|
|
if (dt >= (1 / this->current_speed_) * 1e6f) {
|
|
int32_t mag = this->target_position > this->current_position ? 1 : -1;
|
|
this->last_step_ = now;
|
|
this->current_position += mag;
|
|
return mag;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
} // namespace stepper
|
|
} // namespace esphome
|