25 lines
722 B
C
25 lines
722 B
C
 
|
#pragma once
|
||
|
|
|
||
|
|
// Rate-limited setpoint smoother. Every place a discrete command can jump
|
||
|
|
// (body pose axes, gait speed, the sit/stand blend) gets one of these instead
|
||
|
|
// of using the raw commanded value directly.
|
||
|
|
struct SmoothedValue {
|
||
|
|
float current = 0;
|
||
|
|
float target = 0;
|
||
|
|
float max_rate; // units per second
|
||
|
|
|
||
|
|
explicit SmoothedValue(float rate = 100.0f) : max_rate(rate) {}
|
||
|
|
|
||
|
|
void setTarget(float t) { target = t; }
|
||
|
|
|
||
|
|
float update(float dt) {
|
||
|
|
float delta = target - current;
|
||
|
|
float max_step = max_rate * dt;
|
||
|
|
if (delta > max_step) delta = max_step;
|
||
|
|
else if (delta < -max_step) delta = -max_step;
|
||
|
|
current += delta;
|
||
|
|
return current;
|
||
|
|
}
|
||
|
|
|
||
|
|
void reset(float v) { current = target = v; }
|
||
|
|
};
|