Calibration for ESP32 joysticks.

Author: Mysticial

SerialPrograms/Source/NintendoSwitch/Controllers/SerialPABotBase/NintendoSwitch_SerialPABotBase_WirelessController.h

@@ -10,6 +10,10 @@
 #include "Common/NintendoSwitch/NintendoSwitch_Protocol_ESP32.h"
 #include "NintendoSwitch_SerialPABotBase_Controller.h"
 
+//#include <iostream>
+//using std::cout;
+//using std::endl;
+
 namespace PokemonAutomation{
 namespace NintendoSwitch{
 
@@ -40,6 +44,37 @@ class SerialPABotBase_WirelessController : public SerialPABotBase_Controller{
 
 
 protected:
+    void encode_joystick(uint8_t data[3], uint8_t x, uint8_t y){
+        const uint16_t min = 1897;
+        const uint16_t max = 320;
+
+        const double SHIFT = 2048 - min;
+        const double RATIO = (min - max) / 127.;
+
+        double dx = x - 128.;
+        double dy = y - 128.;
+
+        dx *= RATIO;
+        dy *= RATIO;
+
+        if (dx != 0){
+            dx += dx >= 0 ? SHIFT : -SHIFT;
+        }
+        if (dy != 0){
+            dy += dy >= 0 ? SHIFT : -SHIFT;
+        }
+
+        uint16_t wx = (uint16_t)(2048 + dx + 0.5);
+        uint16_t wy = (uint16_t)(2048 - dy + 0.5);
+
+//        wx = 320;
+//        cout << "wx = " << wx << endl;
+
+        data[0] = (uint8_t)wx;
+        data[1] = (uint8_t)(wx >> 8 | wy << 4);
+        data[2] = (uint8_t)(wy >> 4);
+    }
+
     void issue_report(
         const Cancellable* cancellable,
         const ESP32Report0x30& report,

SerialPrograms/Source/NintendoSwitch/Controllers/SerialPABotBase/NintendoSwitch_SerialPABotBase_WirelessJoycon.cpp

@@ -137,12 +137,8 @@ void SerialPABotBase_WirelessJoycon::push_state_left_joycon(const Cancellable* c
         .button3 = 0,
         .button4 = 0,
         .button5 = 0,
-        .leftstick_x_lo = 0x00,
-        .leftstick_x_hi = 0x08,
-        .leftstick_y = 0x80,
-        .rightstick_x_lo = 0x00,
-        .rightstick_x_hi = 0x08,
-        .rightstick_y = 0x80,
+        .left_joystick = {0x00, 0x08, 0x80},
+        .right_joystick = {0x00, 0x08, 0x80},
         .vibrator = 0x00,
         .gyro = {},
     };
@@ -190,9 +186,7 @@ void SerialPABotBase_WirelessJoycon::push_state_left_joycon(const Cancellable* c
 
     //  Left Stick
     if (m_left_joystick.is_busy()){
-        report.leftstick_x_lo = (m_left_joystick.x << 4) & 0xf0;
-        report.leftstick_x_hi = (m_left_joystick.x & 0xf0) >> 4;
-        report.leftstick_y = 255 - m_left_joystick.y;
+        encode_joystick(report.left_joystick, m_left_joystick.x, m_left_joystick.y);
     }
 
     issue_report(cancellable, report, duration);
@@ -207,12 +201,8 @@ void SerialPABotBase_WirelessJoycon::push_state_right_joycon(const Cancellable*
         .button3 = 0,
         .button4 = 0,
         .button5 = 0,
-        .leftstick_x_lo = 0x00,
-        .leftstick_x_hi = 0x08,
-        .leftstick_y = 0x80,
-        .rightstick_x_lo = 0x00,
-        .rightstick_x_hi = 0x08,
-        .rightstick_y = 0x80,
+        .left_joystick = {0x00, 0x08, 0x80},
+        .right_joystick = {0x00, 0x08, 0x80},
         .vibrator = 0x00,
         .gyro = {},
     };
@@ -257,9 +247,7 @@ void SerialPABotBase_WirelessJoycon::push_state_right_joycon(const Cancellable*
 
     //  Right Stick
     if (m_right_joystick.is_busy()){
-        report.rightstick_x_lo = (m_right_joystick.x << 4) & 0xf0;
-        report.rightstick_x_hi = (m_right_joystick.x & 0xf0) >> 4;
-        report.rightstick_y = 255 - m_right_joystick.y;
+        encode_joystick(report.right_joystick, m_right_joystick.x, m_right_joystick.y);
     }
 
     issue_report(cancellable, report, duration);

SerialPrograms/Source/NintendoSwitch/Controllers/SerialPABotBase/NintendoSwitch_SerialPABotBase_WirelessProController.cpp

@@ -43,12 +43,8 @@ void SerialPABotBase_WirelessProController::push_state(const Cancellable* cancel
         .button3 = 0,
         .button4 = 0,
         .button5 = 0,
-        .leftstick_x_lo = 0x00,
-        .leftstick_x_hi = 0x08,
-        .leftstick_y = 0x80,
-        .rightstick_x_lo = 0x00,
-        .rightstick_x_hi = 0x08,
-        .rightstick_y = 0x80,
+        .left_joystick = {0x00, 0x08, 0x80},
+        .right_joystick = {0x00, 0x08, 0x80},
         .vibrator = 0x00,
         .gyro = {},
     };
@@ -102,16 +98,12 @@ void SerialPABotBase_WirelessProController::push_state(const Cancellable* cancel
 
     //  Left Stick
     if (m_left_joystick.is_busy()){
-        report.leftstick_x_lo = (m_left_joystick.x << 4) & 0xf0;
-        report.leftstick_x_hi = (m_left_joystick.x & 0xf0) >> 4;
-        report.leftstick_y = 255 - m_left_joystick.y;
+        encode_joystick(report.left_joystick, m_left_joystick.x, m_left_joystick.y);
     }
 
     //  Right Stick
     if (m_right_joystick.is_busy()){
-        report.rightstick_x_lo = (m_right_joystick.x << 4) & 0xf0;
-        report.rightstick_x_hi = (m_right_joystick.x & 0xf0) >> 4;
-        report.rightstick_y = 255 - m_right_joystick.y;
+        encode_joystick(report.right_joystick, m_right_joystick.x, m_right_joystick.y);
     }
 
     issue_report(cancellable, report, duration);