timer.cpp 867 B

1234567891011121314151617181920212223242526272829303132333435
  1. #include "timer.h"
  2. #include <algorithm>
  3. #include <thread>
  4. Timer::Timer() :
  5. target_time_{std::chrono::high_resolution_clock::now()} {
  6. }
  7. void Timer::wait(const int64_t period) {
  8. target_time_ += std::chrono::microseconds{period};
  9. const auto lag =
  10. std::chrono::duration_cast<std::chrono::microseconds>(
  11. target_time_ - std::chrono::high_resolution_clock::now()) +
  12. std::chrono::microseconds{adjust()};
  13. std::this_thread::sleep_for(lag);
  14. const int64_t error =
  15. std::chrono::duration_cast<std::chrono::microseconds>(
  16. std::chrono::high_resolution_clock::now() - target_time_).count();
  17. derivative_ = error - proportional_;
  18. integral_ += error;
  19. proportional_ = error;
  20. }
  21. void Timer::update() {
  22. target_time_ = std::chrono::high_resolution_clock::now();
  23. }
  24. int64_t Timer::adjust() const {
  25. return P_ * proportional_ + I_ * integral_ + D_ * derivative_;
  26. }