glider.ino 1.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960
  1. #include <cmath>
  2. #include "glider.h"
  3. #include "math.h"
  4. Glider::Glider()
  5. : speed(0),
  6. sustain(0),
  7. release(0),
  8. error(0)
  9. {}
  10. void Glider::setDirection(int8_t direction) {
  11. if (this->direction != direction) {
  12. stop();
  13. }
  14. this->direction = direction;
  15. }
  16. void Glider::update(float speed, uint16_t sustain) {
  17. this->speed = speed;
  18. this->sustain = sustain;
  19. this->release = sustain;
  20. }
  21. void Glider::updateSpeed(float speed) {
  22. this->speed = speed;
  23. }
  24. void Glider::stop() {
  25. this->speed = 0;
  26. this->sustain = 0;
  27. this->release = 0;
  28. this->error = 0;
  29. }
  30. Glider::GlideResult Glider::glide(millis_t delta) {
  31. const auto alreadyStopped = speed == 0;
  32. error += speed * delta;
  33. int8_t distance = 0;
  34. if (error > 0) {
  35. distance = clamp<int8_t>(std::ceil(error));
  36. }
  37. error -= distance;
  38. if (sustain > 0) {
  39. const auto sustained = min(sustain, (uint16_t)delta);
  40. sustain -= sustained;
  41. } else if (release > 0) {
  42. const auto released = min(release, (uint16_t)delta);
  43. speed = speed * (release - released) / release;
  44. release -= released;
  45. } else {
  46. speed = 0;
  47. }
  48. const int8_t result = direction * distance;
  49. return GlideResult { result, !alreadyStopped && speed == 0 };
  50. }