Calibrate the ESP32 joysticks for real this time. (hopefully)

Mysticial · Mysticial · commit adb165f42482 · 2025-03-16T04:32:18.000-07:00
diff --git a/SerialPrograms/Source/Controllers/JoystickTools.h b/SerialPrograms/Source/Controllers/JoystickTools.h
@@ -14,6 +14,35 @@ namespace PokemonAutomation{
 namespace JoystickTools{
 
 
+inline void max_out_magnitude(double& x, double& y){
+    double mag = x*x + y*y;
+    if (mag == 0){
+        return;
+    }
+
+    double abs_x = std::abs(x);
+    double abs_y = std::abs(y);
+
+    if (abs_x < abs_y){
+        x /= abs_y;
+        y = y < 0 ? -1 : 1;
+    }else{
+        y /= abs_x;
+        x = x < 0 ? -1 : 1;
+    }
+}
+inline double project_to_range(double x, double lo, double hi){
+    if (x <= -1){
+        return -1;
+    }
+    if (x >= 1){
+        return 1;
+    }
+    return x * (hi - lo) + lo;
+}
+
+
+
 
 inline double linear_u8_to_float(uint8_t x){
     if (x <= 128){
@@ -55,26 +84,28 @@ inline uint16_t linear_float_to_u16(double f){
         return (uint16_t)(f + 0.5);
     }
 }
-inline uint16_t linear_float_to_u12(double lo, double hi, double f){
-    if (f == 0){
-        return 2048;
-    }else if (f <= 0){
+
+
+
+inline uint16_t linear_float_to_u12(double f){
+    if (f <= 0){
         f = std::max<double>(f, -1);
-//        if (f < -1){ return 0; }
-        f = f * (hi - lo) - lo;
         f = f * 2048 + 2048;
         return (uint16_t)(f + 0.5);
     }else{
         f = std::min<double>(f, +1);
-//        if (f > 1){ return 4095; }
-        f = f * (hi - lo) + lo;
         f = f * 2047 + 2048;
         return (uint16_t)(f + 0.5);
     }
 }
 
 
 
+
+
+
+
+
 }
 }
 #endif
diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/SerialPABotBase/NintendoSwitch_SerialPABotBase_WirelessController.h b/SerialPrograms/Source/NintendoSwitch/Controllers/SerialPABotBase/NintendoSwitch_SerialPABotBase_WirelessController.h
@@ -7,6 +7,7 @@
 #ifndef PokemonAutomation_NintendoSwitch_SerialPABotBase_WirelessController_H
 #define PokemonAutomation_NintendoSwitch_SerialPABotBase_WirelessController_H
 
+#include <cmath>
 #include "Common/SerialPABotBase/SerialPABotBase_Messages_ESP32.h"
 #include "Controllers/JoystickTools.h"
 #include "NintendoSwitch_SerialPABotBase_Controller.h"
@@ -44,6 +45,7 @@ class SerialPABotBase_WirelessController : public SerialPABotBase_Controller{
 
 
 protected:
+    template <uint16_t min_threshold, uint16_t max_threshold>
     void encode_joystick(uint8_t data[3], uint8_t x, uint8_t y){
         //  2048 is the neutral position.
         //
@@ -52,25 +54,46 @@ class SerialPABotBase_WirelessController : public SerialPABotBase_Controller{
         //
         //  ~320 is where it reaches the maximum value.
         //
-        //  It we linearly interpolate between 1897 and 320, we seem to match
+        //  If we linearly interpolate between 1897 and 320, we seem to match
         //  the wired controller's behavior.
         //
         //  I suspect the need to offset by 151 from 2048 -> 1897 is Nintendo's
         //  way to alleviate the joycon drift problem.
-        const uint16_t min = 1897;
+        //
+        //  The values 320 and 1897 are for the pro controller. Joycons are
+        //  slightly different.
+        //
+//        const uint16_t min = 1897;
 //        const uint16_t max = 320;
-        const uint16_t max = 275;   //  REMOVE: TODO: Fix the clipping for real.
+//        const uint16_t max = 275;
 
-        const double lo = 1 - min / 2048.;
-        const double hi = 1 - max / 2048.;
+        constexpr double lo = 1 - min_threshold / 2048.;
+        constexpr double hi = 1 - max_threshold / 2048.;
 
         double fx = JoystickTools::linear_u8_to_float(x);
         double fy = -JoystickTools::linear_u8_to_float(y);
 //        cout << "fx = " << fx << ", fy = " << fy << endl;
+        double mag_squared = fx*fx + fy*fy;
+
+        uint16_t wx, wy;
+        if (mag_squared == 0){
+            wx = 2048;
+            wy = 2048;
+        }else if (mag_squared >= 1){
+            JoystickTools::max_out_magnitude(fx, fy);
+            wx = JoystickTools::linear_float_to_u12(fx);
+            wy = JoystickTools::linear_float_to_u12(fy);
+        }else{
+            double true_mag = std::sqrt(mag_squared);
+            double report_mag = JoystickTools::project_to_range(true_mag, lo, hi);
+            double scale = report_mag / true_mag;
+            wx = JoystickTools::linear_float_to_u12(fx * scale);
+            wy = JoystickTools::linear_float_to_u12(fy * scale);
+        }
 
-        uint16_t wx = JoystickTools::linear_float_to_u12(lo, hi, fx);
-        uint16_t wy = JoystickTools::linear_float_to_u12(lo, hi, fy);
 //        cout << "wx = " << wx << ", wy = " << wy << endl;
+//        wy = 2048;
+//        wx = 260;
 
         data[0] = (uint8_t)wx;
         data[1] = (uint8_t)(wx >> 8 | wy << 4);
diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/SerialPABotBase/NintendoSwitch_SerialPABotBase_WirelessJoycon.cpp b/SerialPrograms/Source/NintendoSwitch/Controllers/SerialPABotBase/NintendoSwitch_SerialPABotBase_WirelessJoycon.cpp
@@ -181,7 +181,10 @@ void SerialPABotBase_WirelessJoycon::push_state_left_joycon(const Cancellable* c
 
     //  Left Stick
     if (m_left_joystick.is_busy()){
-        encode_joystick(report.left_joystick, m_left_joystick.x, m_left_joystick.y);
+        encode_joystick<JOYSTICK_MIN_THRESHOLD, JOYSTICK_MAX_THRESHOLD>(
+            report.left_joystick,
+            m_left_joystick.x, m_left_joystick.y
+        );
     }
 
     issue_report(cancellable, report, duration);
@@ -238,7 +241,10 @@ void SerialPABotBase_WirelessJoycon::push_state_right_joycon(const Cancellable*
 
     //  Right Stick
     if (m_right_joystick.is_busy()){
-        encode_joystick(report.right_joystick, m_right_joystick.x, m_right_joystick.y);
+        encode_joystick<JOYSTICK_MIN_THRESHOLD, JOYSTICK_MAX_THRESHOLD>(
+            report.right_joystick,
+            m_right_joystick.x, m_right_joystick.y
+        );
     }
 
     issue_report(cancellable, report, duration);
diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/SerialPABotBase/NintendoSwitch_SerialPABotBase_WirelessJoycon.h b/SerialPrograms/Source/NintendoSwitch/Controllers/SerialPABotBase/NintendoSwitch_SerialPABotBase_WirelessJoycon.h
@@ -19,6 +19,9 @@ class SerialPABotBase_WirelessJoycon final :
     public JoyconController,
     public SerialPABotBase_WirelessController
 {
+    static constexpr uint16_t JOYSTICK_MIN_THRESHOLD = 1873;
+    static constexpr uint16_t JOYSTICK_MAX_THRESHOLD = 260;
+
 public:
     SerialPABotBase_WirelessJoycon(
         Logger& logger,
diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/SerialPABotBase/NintendoSwitch_SerialPABotBase_WirelessProController.cpp b/SerialPrograms/Source/NintendoSwitch/Controllers/SerialPABotBase/NintendoSwitch_SerialPABotBase_WirelessProController.cpp
@@ -95,12 +95,18 @@ void SerialPABotBase_WirelessProController::push_state(const Cancellable* cancel
 
     //  Left Stick
     if (m_left_joystick.is_busy()){
-        encode_joystick(report.left_joystick, m_left_joystick.x, m_left_joystick.y);
+        encode_joystick<JOYSTICK_MIN_THRESHOLD, JOYSTICK_MAX_THRESHOLD>(
+            report.left_joystick,
+            m_left_joystick.x, m_left_joystick.y
+        );
     }
 
     //  Right Stick
     if (m_right_joystick.is_busy()){
-        encode_joystick(report.right_joystick, m_right_joystick.x, m_right_joystick.y);
+        encode_joystick<JOYSTICK_MIN_THRESHOLD, JOYSTICK_MAX_THRESHOLD>(
+            report.right_joystick,
+            m_right_joystick.x, m_right_joystick.y
+        );
     }
 
     issue_report(cancellable, report, duration);
diff --git a/SerialPrograms/Source/NintendoSwitch/Controllers/SerialPABotBase/NintendoSwitch_SerialPABotBase_WirelessProController.h b/SerialPrograms/Source/NintendoSwitch/Controllers/SerialPABotBase/NintendoSwitch_SerialPABotBase_WirelessProController.h
@@ -19,6 +19,9 @@ class SerialPABotBase_WirelessProController final :
     public ProController,
     public SerialPABotBase_WirelessController
 {
+    static constexpr uint16_t JOYSTICK_MIN_THRESHOLD = 1897;
+    static constexpr uint16_t JOYSTICK_MAX_THRESHOLD = 320;
+
 public:
     SerialPABotBase_WirelessProController(
         Logger& logger,
diff --git a/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconProgram.cpp b/SerialPrograms/Source/NintendoSwitch/DevPrograms/JoyconProgram.cpp
@@ -45,14 +45,17 @@ void JoyconProgram::program(SingleSwitchProgramEnvironment& env, CancellableScop
     //  No support for gyro yet. That's coming later.
     //
 
+//    pbf_move_joystick(context, 64, 64, 10000ms, 0ms);
+
+#if 1
     pbf_press_button(context, BUTTON_A, 200ms, 2000ms);
     pbf_press_button(context, BUTTON_HOME, 200ms, 2000ms);
     pbf_move_joystick(context, 128, 0, 100ms, 100ms);
     pbf_move_joystick(context, 128, 0, 100ms, 100ms);
     pbf_move_joystick(context, 255, 128, 100ms, 100ms);
     pbf_move_joystick(context, 128, 0, 100ms, 100ms);
     pbf_press_button(context, BUTTON_X, 200ms, 2000ms);
-
+#endif
 
 }
 
diff --git a/SerialPrograms/Source/NintendoSwitch/DevPrograms/TestProgramSwitch.cpp b/SerialPrograms/Source/NintendoSwitch/DevPrograms/TestProgramSwitch.cpp
@@ -312,6 +312,7 @@ void TestProgram::program(MultiSwitchProgramEnvironment& env, CancellableScope&
 
 //    std::terminate();
 
+#if 0
     for (size_t i = 0; i < 100; i++){
         for (size_t c = 0; c < 2; c++){
             ssf_issue_scroll(context, DPAD_RIGHT, 32ms);
@@ -323,7 +324,6 @@ void TestProgram::program(MultiSwitchProgramEnvironment& env, CancellableScope&
 
 
 
-#if 0
     for (size_t c = 0; c < 60; c++){
         ssf_issue_scroll(context, DPAD_DOWN, 20ms);
     }
@@ -333,7 +333,7 @@ void TestProgram::program(MultiSwitchProgramEnvironment& env, CancellableScope&
     }
 #endif
 
-//    pbf_move_left_joystick(context, 0, 0, 10000, 0);
+    pbf_move_left_joystick(context, 38, 38, 10000, 0);
 
 
 //    ssf_issue_scroll(context, DPAD_LEFT, 0);
