From fd8b73a88d8ed33a31e8b4f6651ca31c5b78f7e8 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Mon, 17 Jun 2024 20:30:30 -0700 Subject: [PATCH 1/3] Fix deadband --- src/balancer.hpp | 49 +++++++++++++++++++++++++----------------------- 1 file changed, 26 insertions(+), 23 deletions(-) diff --git a/src/balancer.hpp b/src/balancer.hpp index 722ebbb..9cfaf00 100644 --- a/src/balancer.hpp +++ b/src/balancer.hpp @@ -2,6 +2,7 @@ #include +#include #include #include "bot_can.hpp" @@ -199,6 +200,27 @@ struct BotController { return next_state; } + static float deadband(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 * cmd_abs * cmd_abs; + // restore sign + cmd = copysignf(cmd_abs, cmd); + return cmd; + } + // Commands assumed to be in range [-1, 1] // Outputs scaled to robot units [m/s, rad/s] CmdPair filterConditionCmds(const CmdPair& rx) { @@ -208,29 +230,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 = deadband(rx.drive) * drive_range, + .steer = deadband(rx.steer) * steer_range, + }; // Lowpass filters (tau set in LPF constructor) return { From 0143c1cdab95469f5e5637ffc603111f932511e1 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Mon, 17 Jun 2024 20:35:31 -0700 Subject: [PATCH 2/3] Cleanup deadband, make it remap_joystick --- src/balancer.hpp | 25 ++----------------------- src/utils.hpp | 24 ++++++++++++++++++++++++ 2 files changed, 26 insertions(+), 23 deletions(-) diff --git a/src/balancer.hpp b/src/balancer.hpp index 9cfaf00..3757e48 100644 --- a/src/balancer.hpp +++ b/src/balancer.hpp @@ -200,27 +200,6 @@ struct BotController { return next_state; } - static float deadband(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 * cmd_abs * cmd_abs; - // restore sign - cmd = copysignf(cmd_abs, cmd); - return cmd; - } - // Commands assumed to be in range [-1, 1] // Outputs scaled to robot units [m/s, rad/s] CmdPair filterConditionCmds(const CmdPair& rx) { @@ -231,8 +210,8 @@ struct BotController { // Deadband then, // "expo curve" 2nd order map (quadratic) (with abs to keep sign) const CmdPair mapped_cmd = { - .drive = deadband(rx.drive) * drive_range, - .steer = deadband(rx.steer) * steer_range, + .drive = bot::remap_joystick(rx.drive) * drive_range, + .steer = bot::remap_joystick(rx.steer) * steer_range, }; // Lowpass filters (tau set in LPF constructor) diff --git a/src/utils.hpp b/src/utils.hpp index 1109c81..dd08ef1 100644 --- a/src/utils.hpp +++ b/src/utils.hpp @@ -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; From 27512852c9b30a6b3c6ac4a68a59b5c848c317ac Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Mon, 17 Jun 2024 20:35:58 -0700 Subject: [PATCH 3/3] Remove unused include --- src/balancer.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/balancer.hpp b/src/balancer.hpp index 3757e48..8db1a68 100644 --- a/src/balancer.hpp +++ b/src/balancer.hpp @@ -2,7 +2,6 @@ #include -#include #include #include "bot_can.hpp"