Skip to content

Fix deadband #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Wetmelon wants to merge 3 commits into
base: micro-bb
Choose a base branch
from
Open

Fix deadband #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
fd8b73a
Fix deadband
Wetmelon Jun 18, 2024
0143c1c
Cleanup deadband, make it remap_joystick
Wetmelon Jun 18, 2024
2751285
Remove unused include
Wetmelon Jun 18, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 4 additions & 23 deletions src/balancer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -208,29 +208,10 @@ struct BotController {

// Deadband then,
// "expo curve" 2nd order map (quadratic) (with abs to keep sign)
CmdPair mapped_cmd = rx;
float k2 = 0.5f;
float k1 = 1.0f - k2;
float deadband_each = 0.01f; // total deadband width is 2*deadband_each
for (float cmd : {mapped_cmd.drive, mapped_cmd.steer}) {
// calc in absolute value, copy sign at the end
// deadband
float cmd_abs = fabsf(cmd);
if (cmd_abs < deadband_each) {
cmd_abs = 0.0f;
} else {
// remap from [deadband, 1] to [0, 1]
cmd_abs = (cmd_abs - deadband_each) / (1.0f - deadband_each);
}
// quadratic map
cmd_abs = k1 * cmd_abs + k2 * cmd_abs * cmd_abs;
// restore sign
cmd = copysignf(cmd_abs, cmd);
}

// Scale units
mapped_cmd.drive *= drive_range;
mapped_cmd.steer *= steer_range;
const CmdPair mapped_cmd = {
.drive = bot::remap_joystick(rx.drive) * drive_range,
.steer = bot::remap_joystick(rx.steer) * steer_range,
};

// Lowpass filters (tau set in LPF constructor)
return {
Expand Down
24 changes: 24 additions & 0 deletions src/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,30 @@ constexpr const T& clamp(const T& x, const T& lo, const T& hi) {
return std::min(std::max(x, lo), hi);
}

float remap_joystick(float cmd) {
float k2 = 0.5f;
float k1 = 1.0f - k2;
float deadband_each = 0.01f; // total deadband width is 2*deadband_each
// calc in absolute value, copy sign at the end

// deadband
float cmd_abs = fabsf(cmd);
if (cmd_abs < deadband_each) {
cmd_abs = 0.0f;
} else {
// remap from [deadband, 1] to [0, 1]
cmd_abs = (cmd_abs - deadband_each) / (1.0f - deadband_each);
}

// quadratic map
cmd_abs = (k1 * cmd_abs) + k2 * sq(cmd_abs);

// restore sign
cmd = copysignf(cmd_abs, cmd);

return cmd;
}

void blink(const uint32_t blink_period_ms) {
static uint32_t last_blink = millis();
static bool led_state = false;
Expand Down