-
Notifications
You must be signed in to change notification settings - Fork 46
/
timer.cpp
34 lines (26 loc) · 867 Bytes
/
timer.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
#include "timer.h"
#include <algorithm>
#include <thread>
Timer::Timer() :
target_time_{std::chrono::high_resolution_clock::now()} {
}
void Timer::wait(const int64_t period) {
target_time_ += std::chrono::microseconds{period};
const auto lag =
std::chrono::duration_cast<std::chrono::microseconds>(
target_time_ - std::chrono::high_resolution_clock::now()) +
std::chrono::microseconds{adjust()};
std::this_thread::sleep_for(lag);
const int64_t error =
std::chrono::duration_cast<std::chrono::microseconds>(
std::chrono::high_resolution_clock::now() - target_time_).count();
derivative_ = error - proportional_;
integral_ += error;
proportional_ = error;
}
void Timer::update() {
target_time_ = std::chrono::high_resolution_clock::now();
}
int64_t Timer::adjust() const {
return P_ * proportional_ + I_ * integral_ + D_ * derivative_;
}