From 1cc408eb70d5068855db461e117d93d21f8aaeee Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Thu, 20 Jun 2024 13:07:54 -0500 Subject: [PATCH] feat(math): Refactor `espp::RangeMapper<>` to have center deadband and range deadband and remove invert-input. Similar update to `espp::Joystick`. Fixed bug in `espp::Joystick` which introduced non-linearity when configured as a `CIRCULAR` joystick. (#261) * feat(math): Refactor range mapper to have center deadband and range deadband; remove invert-input * feat(joystick): update joystick to have range deadzone that operates on the vector similar to the rangemapper range deadzone. * feat(joystick): update circularization and add test * Refactor joystick to allow the get function to be null and instead to use the .update(raw_x, raw_y) function, and use new protected recalculate(raw_x, raw_y) function in both the update overloads * Fix bug in circular joystick scaling which inadvertently squared the magnitude as opposed to setting the new magnitude * Update joystick example to loop through array of raw values and print out the circular and square joystick outputs for verification * fix(joystick): update doc and add missing storage of range_deadzone * fix doc * doc: update * final doc update --- .../example/main/controller_example.cpp | 34 ++-- .../joystick/example/main/Kconfig.projbuild | 50 ++++++ .../example/main/joystick_example.cpp | 163 +++++++++++------- components/joystick/include/joystick.hpp | 139 ++++++++++----- components/math/example/main/math_example.cpp | 58 +++---- components/math/include/range_mapper.hpp | 148 ++++++++-------- 6 files changed, 375 insertions(+), 217 deletions(-) create mode 100644 components/joystick/example/main/Kconfig.projbuild diff --git a/components/controller/example/main/controller_example.cpp b/components/controller/example/main/controller_example.cpp index c10d280e1..8286ec0dd 100644 --- a/components/controller/example/main/controller_example.cpp +++ b/components/controller/example/main/controller_example.cpp @@ -76,10 +76,10 @@ extern "C" void app_main(void) { std::vector channels{ {.unit = ADC_UNIT_2, .channel = ADC_CHANNEL_1, // (x) Analog 0 on the joystick shield - .attenuation = ADC_ATTEN_DB_11}, + .attenuation = ADC_ATTEN_DB_12}, {.unit = ADC_UNIT_2, .channel = ADC_CHANNEL_2, // (y) Analog 1 on the joystick shield - .attenuation = ADC_ATTEN_DB_11}}; + .attenuation = ADC_ATTEN_DB_12}}; espp::OneshotAdc adc(espp::OneshotAdc::Config{ .unit = ADC_UNIT_2, .channels = channels, @@ -111,11 +111,16 @@ extern "C" void app_main(void) { .gpio_start = 42, // D4 on the joystick shield .gpio_select = 21, // D6 on the joystick shield .gpio_joystick_select = -1, // D2 on the joystick shield - .joystick_config = - {.x_calibration = {.center = 0.0f, .deadband = 0.2f, .minimum = -1.0f, .maximum = 1.0f}, - .y_calibration = {.center = 0.0f, .deadband = 0.2f, .minimum = -1.0f, .maximum = 1.0f}, - .get_values = read_joystick, - .log_level = espp::Logger::Verbosity::WARN}, + .joystick_config = {.x_calibration = {.center = 0.0f, + .center_deadband = 0.2f, + .minimum = -1.0f, + .maximum = 1.0f}, + .y_calibration = {.center = 0.0f, + .center_deadband = 0.2f, + .minimum = -1.0f, + .maximum = 1.0f}, + .get_values = read_joystick, + .log_level = espp::Logger::Verbosity::WARN}, .log_level = espp::Logger::Verbosity::WARN}); // and finally, make the task to periodically poll the controller and print // the state @@ -236,11 +241,16 @@ extern "C" void app_main(void) { .gpio_start = 42, // pin 37 on the joybonnet .gpio_select = 21, // pin 38 on the joybonnet .gpio_joystick_select = -1, - .joystick_config = - {.x_calibration = {.center = 0.0f, .deadband = 0.2f, .minimum = -1.0f, .maximum = 1.0f}, - .y_calibration = {.center = 0.0f, .deadband = 0.2f, .minimum = -1.0f, .maximum = 1.0f}, - .get_values = read_joystick, - .log_level = espp::Logger::Verbosity::WARN}, + .joystick_config = {.x_calibration = {.center = 0.0f, + .center_deadband = 0.2f, + .minimum = -1.0f, + .maximum = 1.0f}, + .y_calibration = {.center = 0.0f, + .center_deadband = 0.2f, + .minimum = -1.0f, + .maximum = 1.0f}, + .get_values = read_joystick, + .log_level = espp::Logger::Verbosity::WARN}, .log_level = espp::Logger::Verbosity::WARN}); // and finally, make the task to periodically poll the controller and print // the state diff --git a/components/joystick/example/main/Kconfig.projbuild b/components/joystick/example/main/Kconfig.projbuild new file mode 100644 index 000000000..d5f2933f1 --- /dev/null +++ b/components/joystick/example/main/Kconfig.projbuild @@ -0,0 +1,50 @@ +menu "Example Configuration" + + choice EXAMPLE_HARDWARE + prompt "Hardware" + default EXAMPLE_HARDWARE_QTPYPICO + help + Select the hardware to run this example on. + + config EXAMPLE_HARDWARE_QTPYPICO + depends on IDF_TARGET_ESP32 + bool "Qt Py PICO" + + config EXAMPLE_HARDWARE_QTPYS3 + depends on IDF_TARGET_ESP32S3 + bool "Qt Py S3" + + config EXAMPLE_HARDWARE_CUSTOM + bool "Custom" + endchoice + + config EXAMPLE_ADC_UNIT + int "Example ADC Unit" + default 2 if EXAMPLE_HARDWARE_QTPYPICO + default 2 if EXAMPLE_HARDWARE_QTPYS3 + range 1 2 + help + The ADC unit number for the sensor. The ESP32 has two ADC units, + ADC_UNIT_1 and ADC_UNIT_2. Default is ADC UNIT 2. + + config EXAMPLE_ADC_CHANNEL_X + int "Joystick X Axis ADC Channel" + range 0 50 + default 9 if EXAMPLE_HARDWARE_QTPYPICO + default 7 if EXAMPLE_HARDWARE_QTPYS3 + default 0 if EXAMPLE_HARDWARE_CUSTOM + help + ADC Channel for the X axis of the joystick. Default is ADC2 channel + 9 (A0) for Qt Py PICO and ADC2 channel 7 (A0) for Qt Py ESP32S3. + + config EXAMPLE_ADC_CHANNEL_Y + int "Joystick Y Axis ADC Channel" + range 0 50 + default 8 if EXAMPLE_HARDWARE_QTPYPICO + default 6 if EXAMPLE_HARDWARE_QTPYS3 + default 1 if EXAMPLE_HARDWARE_CUSTOM + help + ADC Channel for the Y axis of the joystick. Default is ADC2 channel + 8 (A1) for Qt Py PICO and ADC2 channel 6 (A1) for Qt Py ESP32S3. + +endmenu diff --git a/components/joystick/example/main/joystick_example.cpp b/components/joystick/example/main/joystick_example.cpp index 08f2a2b16..6d6fc1832 100644 --- a/components/joystick/example/main/joystick_example.cpp +++ b/components/joystick/example/main/joystick_example.cpp @@ -10,66 +10,113 @@ using namespace std::chrono_literals; extern "C" void app_main(void) { { fmt::print("Running joystick example\n"); - //! [adc joystick example] - std::vector channels{{.unit = ADC_UNIT_2, - .channel = ADC_CHANNEL_9, // Qt Py ESP32 PICO A0 - .attenuation = ADC_ATTEN_DB_11}, - {.unit = ADC_UNIT_2, - .channel = ADC_CHANNEL_8, // Qt Py ESP32 PICO A1 - .attenuation = ADC_ATTEN_DB_11}}; - espp::OneshotAdc adc({ - .unit = ADC_UNIT_2, - .channels = channels, - }); - auto read_joystick = [&adc, &channels](float *x, float *y) -> bool { - // this will be in mv - auto maybe_x_mv = adc.read_mv(channels[0]); - auto maybe_y_mv = adc.read_mv(channels[1]); - if (maybe_x_mv.has_value() && maybe_y_mv.has_value()) { - auto x_mv = maybe_x_mv.value(); - auto y_mv = maybe_y_mv.value(); - *x = (float)(x_mv); - *y = (float)(y_mv); - return true; + { + //! [circular joystick example] + float min = 0; + float max = 255; + float center = 127; + float deadband_percent = 0.1; + float deadband = deadband_percent * (max - min); + + // circular joystick + espp::Joystick js1({ + .x_calibration = {.center = center, .minimum = min, .maximum = max}, + .y_calibration = {.center = center, .minimum = min, .maximum = max}, + .type = espp::Joystick::Type::CIRCULAR, + .center_deadzone_radius = deadband_percent, + .range_deadzone = deadband_percent, + }); + // square joystick (for comparison) + espp::Joystick js2({ + .x_calibration = {.center = center, + .center_deadband = deadband, + .minimum = min, + .maximum = max, + .range_deadband = deadband}, + .y_calibration = {.center = center, + .center_deadband = deadband, + .minimum = min, + .maximum = max, + .range_deadband = deadband}, + }); + // now make a loop where we update the raw valuse and print out the joystick values + fmt::print("raw x, raw y, js1 x, js1 y, js2 x, js2 y\n"); + for (float x = min - 10.0f; x <= max + 10.0f; x += 10.0f) { + for (float y = min - 10.0f; y <= max + 10.0f; y += 10.0f) { + js1.update(x, y); + js2.update(x, y); + fmt::print("{}, {}, {}, {}, {}, {}\n", x, y, js1.x(), js1.y(), js2.x(), js2.y()); + } } - return false; - }; - espp::Joystick js1({ - // convert [0, 3300]mV to approximately [-1.0f, 1.0f] - .x_calibration = - {.center = 1700.0f, .deadband = 100.0f, .minimum = 0.0f, .maximum = 3300.0f}, - .y_calibration = - {.center = 1700.0f, .deadband = 100.0f, .minimum = 0.0f, .maximum = 3300.0f}, - .get_values = read_joystick, - }); - espp::Joystick js2({ - // convert [0, 3300]mV to approximately [-1.0f, 1.0f] - .x_calibration = {.center = 1700.0f, .deadband = 0.0f, .minimum = 0.0f, .maximum = 3300.0f}, - .y_calibration = {.center = 1700.0f, .deadband = 0.0f, .minimum = 0.0f, .maximum = 3300.0f}, - .type = espp::Joystick::Type::CIRCULAR, - .center_deadzone_radius = 0.1f, - .get_values = read_joystick, - }); - auto task_fn = [&js1, &js2](std::mutex &m, std::condition_variable &cv) { - js1.update(); - js2.update(); - fmt::print("{}, {}\n", js1, js2); - // NOTE: sleeping in this way allows the sleep to exit early when the - // task is being stopped / destroyed - { - std::unique_lock lk(m); - cv.wait_for(lk, 500ms); + //! [circular joystick example] + } + { + //! [adc joystick example] + static constexpr adc_unit_t ADC_UNIT = CONFIG_EXAMPLE_ADC_UNIT == 1 ? ADC_UNIT_1 : ADC_UNIT_2; + static constexpr adc_channel_t ADC_CHANNEL_X = (adc_channel_t)CONFIG_EXAMPLE_ADC_CHANNEL_X; + static constexpr adc_channel_t ADC_CHANNEL_Y = (adc_channel_t)CONFIG_EXAMPLE_ADC_CHANNEL_Y; + + std::vector channels{ + {.unit = ADC_UNIT, .channel = ADC_CHANNEL_X, .attenuation = ADC_ATTEN_DB_12}, + {.unit = ADC_UNIT, .channel = ADC_CHANNEL_Y, .attenuation = ADC_ATTEN_DB_12}}; + espp::OneshotAdc adc({ + .unit = ADC_UNIT_2, + .channels = channels, + }); + auto read_joystick = [&adc, &channels](float *x, float *y) -> bool { + // this will be in mv + auto maybe_x_mv = adc.read_mv(channels[0]); + auto maybe_y_mv = adc.read_mv(channels[1]); + if (maybe_x_mv.has_value() && maybe_y_mv.has_value()) { + auto x_mv = maybe_x_mv.value(); + auto y_mv = maybe_y_mv.value(); + *x = (float)(x_mv); + *y = (float)(y_mv); + return true; + } + return false; + }; + espp::Joystick js1({ + // convert [0, 3300]mV to approximately [-1.0f, 1.0f] + .x_calibration = + {.center = 1700.0f, .center_deadband = 100.0f, .minimum = 0.0f, .maximum = 3300.0f}, + .y_calibration = + {.center = 1700.0f, .center_deadband = 100.0f, .minimum = 0.0f, .maximum = 3300.0f}, + .get_values = read_joystick, + }); + espp::Joystick js2({ + // convert [0, 3300]mV to approximately [-1.0f, 1.0f] + .x_calibration = + {.center = 1700.0f, .center_deadband = 0.0f, .minimum = 0.0f, .maximum = 3300.0f}, + .y_calibration = + {.center = 1700.0f, .center_deadband = 0.0f, .minimum = 0.0f, .maximum = 3300.0f}, + .type = espp::Joystick::Type::CIRCULAR, + .center_deadzone_radius = 0.1f, + .get_values = read_joystick, + }); + auto task_fn = [&js1, &js2](std::mutex &m, std::condition_variable &cv) { + js1.update(); + js2.update(); + fmt::print("{}, {}\n", js1, js2); + // NOTE: sleeping in this way allows the sleep to exit early when the + // task is being stopped / destroyed + { + std::unique_lock lk(m); + cv.wait_for(lk, 500ms); + } + // don't want to stop the task + return false; + }; + auto task = espp::Task( + {.name = "Joystick", .callback = task_fn, .log_level = espp::Logger::Verbosity::INFO}); + fmt::print("js1 x, js1 y, js2 x, js2 y\n"); + task.start(); + //! [adc joystick example] + + // loop forever to let the task run and user to play with the joystick + while (true) { + std::this_thread::sleep_for(1s); } - // don't want to stop the task - return false; - }; - auto task = espp::Task( - {.name = "Joystick", .callback = task_fn, .log_level = espp::Logger::Verbosity::INFO}); - fmt::print("js1 x, js1 y, js2 x, js2 y\n"); - task.start(); - //! [adc joystick example] - while (true) { - std::this_thread::sleep_for(1s); } } diff --git a/components/joystick/include/joystick.hpp b/components/joystick/include/joystick.hpp index 41634d4ca..9ab2c1711 100644 --- a/components/joystick/include/joystick.hpp +++ b/components/joystick/include/joystick.hpp @@ -10,7 +10,9 @@ namespace espp { /** * @brief 2-axis Joystick with axis mapping / calibration. * - * \section joystick_ex1 ADC Joystick Example + * \section joystick_ex1 Basic Circular and Rectangular Joystick Example + * \snippet joystick_example.cpp circular joystick example + * \section joystick_ex2 ADC Joystick Example * \snippet joystick_example.cpp adc joystick example */ class Joystick : public BaseComponent { @@ -27,8 +29,9 @@ class Joystick : public BaseComponent { /// [-1,1]) independently which results in x/y deadzones and /// output that are rectangular. CIRCULAR, ///< The joystick is configured to have a circular output. This - /// means that the x/y < deadzones are circular around the input - /// and the output is clamped to be on or within the unit circle. + /// means that the x/y < deadzones are circular around the + /// input and range and the output is clamped to be on or + /// within the unit circle. }; /** @@ -52,8 +55,16 @@ class Joystick : public BaseComponent { float center_deadzone_radius{ 0}; /**< The radius of the unit circle's deadzone [0, 1.0f] around the center, only used when the joystick is configured as Type::CIRCULAR. */ - get_values_fn - get_values; /**< Function to retrieve the latest unmapped joystick values (range [-1,1]). */ + float range_deadzone{0}; /**< The deadzone around the edge of the unit circle, only used when + the joystick is configured as Type::CIRCULAR. This scales the output so + that the output appears to have magnitude 1 (meaning it appears to be on + the edge of the unit circle) when the joystick value magnitude is within + the range [1-range_deadzone, 1]. */ + get_values_fn get_values{nullptr}; /**< Function to retrieve the latest + unmapped joystick values. Required if + you want to use update(), unused if + you call update(float raw_x, float + raw_y). */ Logger::Verbosity log_level{ Logger::Verbosity::WARN}; /**< Verbosity for the Joystick logger_. */ }; @@ -68,6 +79,7 @@ class Joystick : public BaseComponent { , y_mapper_(config.y_calibration) , type_(config.type) , center_deadzone_radius_(config.center_deadzone_radius) + , range_deadzone_(config.range_deadzone) , get_values_(config.get_values) {} /** @@ -77,19 +89,33 @@ class Joystick : public BaseComponent { * Type::CIRCULAR. When the magnitude of the joystick's mapped * position vector is less than this value, the vector is set to * (0,0). + * @param range_deadzone Optional deadzone around the edge of the unit circle + * when \p type is Type::CIRCULAR. This scales the output so that the + * output appears to have magnitude 1 (meaning it appears to be on the + * edge of the unit circle) if the magnitude of the mapped position + * vector is greater than 1-range_deadzone. Example: if the range + * deadzone is 0.1, then the output will be scaled so that the + * magnitude of the output is 1 if the magnitude of the mapped + * position vector is greater than 0.9. * @note If the Joystick is Type::CIRCULAR, the actual calibrations that are - * saved into the joystick will have 0 deadzone around the center - * value, so that only the center deadzone radius is applied. + * saved into the joystick will have 0 deadzone around the center value + * and range values, so that center and range deadzones are actually + * applied on the vector value instead of on the individual axes + * independently. * @sa set_center_deadzone_radius + * @sa set_range_deadzone * @sa set_calibration */ - void set_type(Type type, float radius = 0) { + void set_type(Type type, float radius = 0, float range_deadzone = 0) { type_ = type; if (type_ == Type::CIRCULAR) { - x_mapper_.set_deadband(0); - y_mapper_.set_deadband(0); + x_mapper_.set_center_deadband(0); + y_mapper_.set_center_deadband(0); + x_mapper_.set_range_deadband(0); + y_mapper_.set_range_deadband(0); } center_deadzone_radius_ = radius; + range_deadzone_ = range_deadzone; } /** @@ -102,6 +128,20 @@ class Joystick : public BaseComponent { */ void set_center_deadzone_radius(float radius) { center_deadzone_radius_ = radius; } + /** + * @brief Sets the range deadzone. + * @note Range deadzone is only applied when \p deadzone is Deadzone::CIRCULAR. + * @param range_deadzone Optional deadzone around the edge of the unit circle + * when \p deadzone is Deadzone::CIRCULAR. This scales the output so + * that the output appears to have magnitude 1 (meaning it appears to + * be on the edge of the unit circle) if the magnitude of the mapped + * position vector is greater than 1-range_deadzone. Example: if the + * range deadzone is 0.1, then the output will be scaled so that the + * magnitude of the output is 1 if the magnitude of the mapped position + * vector is greater than 0.9. + */ + void set_range_deadzone(float range_deadzone) { range_deadzone_ = range_deadzone; } + /** * @brief Update the x and y axis mapping. * @param x_calibration New x-axis range mapping configuration to use. @@ -109,26 +149,39 @@ class Joystick : public BaseComponent { * @param center_deadzone_radius The radius of the unit circle's deadzone [0, * 1.0f] around the center, only used when the joystick is configured * as Type::CIRCULAR. + * @param range_deadzone Optional deadzone around the edge of the unit circle + * when \p type is Type::CIRCULAR. This scales the output so that the + * output appears to have magnitude 1 (meaning it appears to be on the + * edge of the unit circle) if the magnitude of the mapped position + * vector is greater than 1-range_deadzone. Example: if the range + * deadzone is 0.1, then the output will be scaled so that the + * magnitude of the output is 1 if the magnitude of the mapped + * position vector is greater than 0.9. * @note If the Joystick is Type::CIRCULAR, the actual calibrations that are - * saved into the joystick will have 0 deadzone around the center value, - * so that only the center deadzone radius is applied. + * saved into the joystick will have 0 deadzone around the center and range values, + * so that center and range deadzones are actually applied on the vector value. * @sa set_center_deadzone_radius + * @sa set_range_deadzone */ void set_calibration(const FloatRangeMapper::Config &x_calibration, const FloatRangeMapper::Config &y_calibration, - float center_deadzone_radius = 0) { + float center_deadzone_radius = 0, float range_deadzone = 0) { x_mapper_.configure(x_calibration); y_mapper_.configure(y_calibration); if (type_ == Type::CIRCULAR) { - x_mapper_.set_deadband(0); - y_mapper_.set_deadband(0); + x_mapper_.set_center_deadband(0); + y_mapper_.set_center_deadband(0); + x_mapper_.set_range_deadband(0); + y_mapper_.set_range_deadband(0); } center_deadzone_radius_ = center_deadzone_radius; + range_deadzone_ = range_deadzone; } /** * @brief Read the raw values and use the calibration data to update the * position. + * @note Requires that the get_values_ function is set. */ void update() { if (!get_values_) { @@ -143,32 +196,19 @@ class Joystick : public BaseComponent { return; } logger_.debug("Got x,y values: ({}, {})", _x, _y); - raw_.x(_x); - raw_.y(_y); - position_.x(x_mapper_.map(_x)); - position_.y(y_mapper_.map(_y)); - // if we're configured to be a Type::CIRCULAR joystick, use the center - // deadzone radius and clamp the output to be within the unit circle - if (type_ == Type::CIRCULAR) { - auto magnitude = position_.magnitude(); - if (magnitude < center_deadzone_radius_) { - // if it's within the deadzone radius, then set both axes to 0. - position_.x(0); - position_.y(0); - } else if (magnitude > 1.0f) { - // if it's outside the unit circle, then normalize the vector so that - // it's on the unit circle - position_ = position_.normalized(); - } else { - // otherwise we should scale the vector so that it's 0 on the edge of - // the deadzone and 1 on the edge of the unit circle - const float magnitude_range = 1.0f - center_deadzone_radius_; - const float scale = (magnitude - center_deadzone_radius_) / magnitude_range; - position_ *= scale; - } - } + recalculate(_x, _y); } + /** + * @brief Update the joystick's position using the provided raw x and y + * values. + * @param raw_x The raw x-axis value. + * @param raw_y The raw y-axis value. + * @note This function is useful when you have the raw values and don't want + * to use the get_values_ function. + */ + void update(float raw_x, float raw_y) { recalculate(raw_x, raw_y); } + /** * @brief Get the most recently updated x axis calibrated position. * @return The most recent x-axis position (from when update() was last @@ -202,12 +242,33 @@ class Joystick : public BaseComponent { friend struct fmt::formatter; protected: + void recalculate(float raw_x, float raw_y) { + raw_.x(raw_x); + raw_.y(raw_y); + position_.x(x_mapper_.map(raw_x)); + position_.y(y_mapper_.map(raw_y)); + if (type_ == Type::CIRCULAR) { + auto magnitude = position_.magnitude(); + if (magnitude < center_deadzone_radius_) { + position_.x(0); + position_.y(0); + } else if (magnitude > 1.0f - range_deadzone_) { + position_ = position_.normalized(); + } else { + const float magnitude_range = 1.0f - center_deadzone_radius_ - range_deadzone_; + const float new_magnitude = (magnitude - center_deadzone_radius_) / magnitude_range; + position_ = position_.normalized() * new_magnitude; + } + } + } + Vector2f raw_{}; Vector2f position_{}; FloatRangeMapper x_mapper_{}; FloatRangeMapper y_mapper_{}; Type type_{Type::RECTANGULAR}; float center_deadzone_radius_{0}; + float range_deadzone_{0}; get_values_fn get_values_{nullptr}; }; } // namespace espp diff --git a/components/math/example/main/math_example.cpp b/components/math/example/main/math_example.cpp index 9b8581d62..8af3ab72c 100644 --- a/components/math/example/main/math_example.cpp +++ b/components/math/example/main/math_example.cpp @@ -121,62 +121,46 @@ extern "C" void app_main(void) { static constexpr float max = 255.0f; // Default will have output range [-1, 1] espp::RangeMapper rm( - {.center = center, .deadband = deadband, .minimum = min, .maximum = max}); + {.center = center, .center_deadband = deadband, .minimum = min, .maximum = max}); // You can explicitly set output center/range. In this case the output will // be in the range [0, 1024] espp::RangeMapper rm2({.center = center, - .deadband = deadband, + .center_deadband = deadband, .minimum = min, .maximum = max, .output_center = 512, .output_range = 512}); - // You can also invert the input distribution, such that input values are - // compared against the input min/max instead of input center. NOTE: this - // also showcases the use of a non-centered input distribution. - espp::FloatRangeMapper rm3({.center = center, - .deadband = deadband, + // You can also use a non-centered input distribution. + espp::FloatRangeMapper rm3({.center = center / 2, + .center_deadband = deadband, .minimum = min, .maximum = max, - .invert_input = true, .output_center = 512, .output_range = 512}); - // You can even invert the ouput distribution + // You can even invert the ouput distribution, and add a deadband around the + // min/max values espp::FloatRangeMapper rm4({ .center = center, - .deadband = deadband, + .center_deadband = deadband, .minimum = min, .maximum = max, + .range_deadband = deadband, .invert_output = true, }); - auto vals = std::vector{min - 10, min, min + 5, min + 10, min + deadband, - // should show as approx -.66 and -.33 - min + (center - deadband - min) * .33f, - min + (center - deadband - min) * .66f, center - deadband, - center - 7, center, center + 7, center + deadband, - // should show as approx .33 and .66 - center + deadband + (max - center - deadband) * .33f, - center + deadband + (max - center - deadband) * .66f, - max - deadband, max - 10, max - 5, max, max + 10}; - // test the mapping and unmapping - fmt::print("Mapping [0, 255] -> [-1, 1]\n"); - fmt::print("% value, mapped, unmapped\n"); - for (const auto &v : vals) { - fmt::print("{}, {}, {}\n", v, rm.map(v), rm.unmap(rm.map(v))); - } - fmt::print("Mapping [0, 255] -> [0, 1024]\n"); - fmt::print("% value, mapped, unmapped\n"); - for (const auto &v : vals) { - fmt::print("{}, {}, {}\n", v, rm2.map(v), rm2.unmap(rm2.map(v))); + // make a vector of float values min - 10 to max + 10 in increments of 5 + std::vector vals; + for (float v = min - 10; v <= max + 10; v += 5) { + vals.push_back(v); } - fmt::print("Mapping Inverted [0, 255] -> [1024, 0]\n"); - fmt::print("% value, mapped, unmapped\n"); - for (const auto &v : vals) { - fmt::print("{}, {}, {}\n", v, rm3.map(v), rm3.unmap(rm3.map(v))); - } - fmt::print("Mapping [0, 255] -> Inverted [1, -1]\n"); - fmt::print("% value, mapped, unmapped\n"); + // test the mapping and unmapping + fmt::print( + "% value, mapped [0;255] to [-1;1], unmapped [-1;1] to [0;255], mapped [0;255] to " + "[0;1024], unmapped [0;1024] to [0;255], mapped [0;255] to [1024;0], unmapped [1024;0] to " + "[0;255], mapped [0;255] to inverted [1;-1], unmapped inverted [1;-1] to [0;255]\n"); for (const auto &v : vals) { - fmt::print("{}, {}, {}\n", v, rm4.map(v), rm4.unmap(rm4.map(v))); + fmt::print("{}, {}, {}, {}, {}, {}, {}, {}, {}\n", v, rm.map(v), rm.unmap(rm.map(v)), + rm2.map(v), rm2.unmap(rm2.map(v)), rm3.map(v), rm3.unmap(rm3.map(v)), rm4.map(v), + rm4.unmap(rm4.map(v))); } //! [range_mapper example] } diff --git a/components/math/include/range_mapper.hpp b/components/math/include/range_mapper.hpp index fb263f7c6..6dc137900 100644 --- a/components/math/include/range_mapper.hpp +++ b/components/math/include/range_mapper.hpp @@ -41,18 +41,14 @@ template class RangeMapper { * and 1 output range provide a default output range between [-1, 1]. */ struct Config { - T center; /**< Center value for the input range. */ - T deadband; /**< Deadband amount around (+-) the center for which output will be 0. */ - T minimum; /**< Minimum value for the input range. */ - T maximum; /**< Maximum value for the input range. */ - bool invert_input{false}; /**< Whether to invert the input distribution (default false). - @note If true will compute the input relative to min/max - instead of to center. @warning This introduces a - discontinuity at the center value and ambiguity around the - min/max values when un-mapping back to the input - distribution. For these reasons this setting is not - recommended and may be replaced in future revisions. */ - T output_center{T(0)}; /**< The center for the output. Default 0. */ + T center; /**< Center value for the input range. */ + T center_deadband{ + T(0)}; /**< Deadband amount around (+-) the center for which output will be 0. */ + T minimum; /**< Minimum value for the input range. */ + T maximum; /**< Maximum value for the input range. */ + T range_deadband{T(0)}; /**< Deadband amount around the minimum and maximum for which output + will be min/max output. */ + T output_center{T(0)}; /**< The center for the output. Default 0. */ T output_range{T(1)}; /**< The range (+/-) from the center for the output. Default 1. @note Will be passed through std::abs() to ensure it is positive. */ bool invert_output{ @@ -85,16 +81,18 @@ template class RangeMapper { } center_ = config.center; - deadband_ = config.deadband; + center_deadband_ = std::abs(config.center_deadband); minimum_ = config.minimum; maximum_ = config.maximum; - invert_input_ = config.invert_input; + range_deadband_ = std::abs(config.range_deadband); output_center_ = config.output_center; output_range_ = std::abs(config.output_range); output_min_ = output_center_ - output_range_; output_max_ = output_center_ + output_range_; - pos_range_ = (maximum_ - (center_ + deadband_)) / output_range_; - neg_range_ = (center_ - deadband_ - minimum_) / output_range_; + // positive range is the range from the (center + center_deadband) to (max - range_deadband) + pos_range_ = (maximum_ - range_deadband_ - (center_ + center_deadband_)) / output_range_; + // negative range is the range from the (center - center_deadband) to (min + range_deadband) + neg_range_ = (center_ - center_deadband_ - (minimum_ + range_deadband_)) / output_range_; invert_output_ = config.invert_output; } @@ -124,13 +122,24 @@ template class RangeMapper { T get_output_max() const { return output_max_; } /** - * @brief Set the deadband for the input distribution. - * @param deadband The deadband to use for the input distribution. + * @brief Set the deadband around the center of the input distribution. + * @param deadband The deadband to use around the center of the input + * distribution. * @note The deadband must be non-negative. * @note The deadband is applied around the center value of the input * distribution. */ - void set_deadband(T deadband) { deadband_ = deadband; } + void set_center_deadband(T deadband) { center_deadband_ = deadband; } + + /** + * @brief Set the deadband around the min/max of the input distribution. + * @param deadband The deadband to use around the min/max of the input + * distribution. + * @note The deadband must be non-negative. + * @note The deadband is applied around the min/max values of the input + * distribution. + */ + void set_range_deadband(T deadband) { range_deadband_ = deadband; } /** * @brief Map a value \p v from the input distribution into the configured @@ -142,31 +151,31 @@ template class RangeMapper { T map(const T &v) const { T clamped = std::clamp(v, minimum_, maximum_); T calibrated{0}; - bool positive_input = clamped >= center_; - if (invert_input_) { - // if we invert the input, then we are comparing against the min/max - calibrated = positive_input ? maximum_ - clamped : minimum_ - clamped; - // if it's within the deadband, return the output center - if (std::abs(calibrated) < deadband_) { - return output_center_; - } - // remove the deadband from the calibrated value - calibrated = positive_input ? calibrated + deadband_ : calibrated - deadband_; - } else { - // normally we compare against center - calibrated = clamped - center_; - // if it's within the deadband, return the output center - if (std::abs(calibrated) < deadband_) { - return output_center_; - } - // remove the deadband from the calibrated value - calibrated = positive_input ? calibrated - deadband_ : calibrated + deadband_; + // compare against center + calibrated = clamped - center_; + bool positive_input = calibrated >= 0; + bool within_center_deadband = std::abs(calibrated) < center_deadband_; + bool within_range_deadband = + clamped >= maximum_ - range_deadband_ || clamped <= minimum_ + range_deadband_; + if (within_center_deadband) { + // if it's within the center deadband, return the output center + return output_center_; + } else if (within_range_deadband) { + // if it's within the range deadband around the min/max, return the output + // min/max, taking into account the output inversion + return positive_input ? invert_output_ ? output_min_ : output_max_ + : invert_output_ ? output_max_ + : output_min_; } - T output = positive_input ? calibrated / pos_range_ + output_center_ - : calibrated / neg_range_ + output_center_; + + // remove the deadband from the calibrated value + calibrated = positive_input ? calibrated - center_deadband_ : calibrated + center_deadband_; + + T output = positive_input ? calibrated / pos_range_ : calibrated / neg_range_; if (invert_output_) { output = -output; } + output += output_center_; return std::clamp(output, output_min_, output_max_); } @@ -175,47 +184,42 @@ template class RangeMapper { * default [-1,1]) back into the input distribution. * @param T&v Value from the centered output distribution. * @return Value within the input distribution. - * @note If `invert_input` is true, then the max/min of the input range both - * map to the output center, which means that unmapping a value at the - * output center is ambiguous. In this case unmap() will return the - * maximum input value. */ T unmap(const T &v) const { T clamped = std::clamp(v, output_min_, output_max_); - T calibrated{0}; + // return early if we're in the center deadband + if (clamped == output_center_) { + return center_; + } + // return early if we're in the range deadband + if (clamped == output_min_ || clamped == output_max_) { + return invert_output_ ? clamped == output_min_ ? (maximum_ - range_deadband_) + : (minimum_ + range_deadband_) + : clamped == output_min_ ? (minimum_ + range_deadband_) + : (maximum_ - range_deadband_); + } + // else we need to convert the output value back to the input range if (invert_output_) { - clamped = -clamped; + // if the output is inverted, we need to invert the output value (flip + // around the output center) + clamped = output_center_ - (clamped - output_center_); } bool positive_output = clamped >= output_center_; + T calibrated{0}; if (positive_output) { - calibrated = (clamped - output_center_) * pos_range_; - } else { - calibrated = (clamped - output_center_) * neg_range_; - } - if (invert_input_) { - if (clamped == output_center_) { - // NOTE: we cannot know if the original input value was minimum or maximum - // so we return the maximum value - return maximum_; - } - calibrated = - positive_output ? maximum_ - calibrated + deadband_ : minimum_ - calibrated + deadband_; + calibrated = (clamped - output_center_) * pos_range_ + center_ + center_deadband_; } else { - if (clamped == output_center_) { - return center_; - } - calibrated = - positive_output ? calibrated + center_ + deadband_ : calibrated + center_ - deadband_; + calibrated = (clamped - output_center_) * neg_range_ + center_ - center_deadband_; } return std::clamp(calibrated, minimum_, maximum_); } protected: T center_{0}; - T deadband_{0}; + T center_deadband_{0}; T minimum_{0}; T maximum_{0}; - bool invert_input_{false}; + T range_deadband_{0}; T pos_range_{1}; T neg_range_{1}; T output_center_{0}; @@ -244,15 +248,17 @@ typedef RangeMapper IntRangeMapper; template <> struct fmt::formatter : fmt::formatter { auto format(const espp::FloatRangeMapper::Config &config, format_context &ctx) { - return fmt::format_to(ctx.out(), "[{},{},{},{},{},{},{},{}]", config.center, config.deadband, - config.minimum, config.maximum, config.invert_input, config.output_center, - config.output_range, config.invert_output); + return fmt::format_to(ctx.out(), "FloatRangeMapper[{},{},{},{},{},{},{},{}]", config.center, + config.center_deadband, config.minimum, config.maximum, + config.range_deadband, config.output_center, config.output_range, + config.invert_output); } }; template <> struct fmt::formatter : fmt::formatter { auto format(const espp::IntRangeMapper::Config &config, format_context &ctx) { - return fmt::format_to(ctx.out(), "[{},{},{},{},{},{},{},{}]", config.center, config.deadband, - config.minimum, config.maximum, config.invert_input, config.output_center, - config.output_range, config.invert_output); + return fmt::format_to(ctx.out(), "IntRangeMapper[{},{},{},{},{},{},{},{}]", config.center, + config.center_deadband, config.minimum, config.maximum, + config.range_deadband, config.output_center, config.output_range, + config.invert_output); } };