31 lines
553 B
C++
31 lines
553 B
C++
#include "switch.h"
|
|
|
|
#include <algorithm>
|
|
#include <cstdio>
|
|
|
|
Switch::Switch()
|
|
: position_(-1) {
|
|
}
|
|
|
|
int Switch::update(const Fixed& fixed, uint16_t level) {
|
|
int lowest = UINT16_MAX;
|
|
int next = -1;
|
|
|
|
for (int i = 0; i < 4; i++) {
|
|
int delta = std::abs(level - fixed.levels[i]);
|
|
|
|
if (i == position_) {
|
|
/* Boost the last position */
|
|
delta -= Hysteresis;
|
|
}
|
|
|
|
if (delta < lowest) {
|
|
next = i;
|
|
lowest = delta;
|
|
}
|
|
}
|
|
|
|
position_ = next;
|
|
return next;
|
|
}
|