Clip joystick magnitude to 1.0.

Author: Mysticial

SerialPrograms/Source/CommonFramework/Globals.cpp

@@ -26,7 +26,7 @@ namespace PokemonAutomation{
 const bool IS_BETA_VERSION = true;
 const int PROGRAM_VERSION_MAJOR = 0;
 const int PROGRAM_VERSION_MINOR = 54;
-const int PROGRAM_VERSION_PATCH = 22;
+const int PROGRAM_VERSION_PATCH = 23;
 
 const std::string PROGRAM_VERSION_BASE =
     "v" + std::to_string(PROGRAM_VERSION_MAJOR) +

SerialPrograms/Source/Controllers/JoystickTools.h

@@ -8,12 +8,24 @@
 #define PokemonAutomation_Controllers_JoystickTools_H
 
 #include <stdint.h>
+#include <cmath>
 #include <algorithm>
 
 namespace PokemonAutomation{
 namespace JoystickTools{
 
 
+inline void clip_magnitude(double& x, double& y){
+    double mag = x*x + y*y;
+    if (mag <= 1.0){
+        return;
+    }
+
+    double scale = 1 / std::sqrt(mag);
+
+    x *= scale;
+    y *= scale;
+}
 inline void max_out_magnitude(double& x, double& y){
     double mag = x*x + y*y;
     if (mag == 0){

SerialPrograms/Source/NintendoSwitch/Controllers/SysbotBase/SysbotBase3_ProController.cpp

@@ -212,12 +212,14 @@ void ProController_SysbotBase3::push_state(const Cancellable* cancellable, WallD
     if (m_left_joystick.is_busy()){
         double fx = JoystickTools::linear_u8_to_float(m_left_joystick.x);
         double fy = -JoystickTools::linear_u8_to_float(m_left_joystick.y);
+        JoystickTools::clip_magnitude(fx, fy);
         left_x = JoystickTools::linear_float_to_s16(fx);
         left_y = JoystickTools::linear_float_to_s16(fy);
     }
     if (m_right_joystick.is_busy()){
         double fx = JoystickTools::linear_u8_to_float(m_right_joystick.x);
         double fy = -JoystickTools::linear_u8_to_float(m_right_joystick.y);
+        JoystickTools::clip_magnitude(fx, fy);
         right_x = JoystickTools::linear_float_to_s16(fx);
         right_y = JoystickTools::linear_float_to_s16(fy);
     }

SerialPrograms/Source/NintendoSwitch/Controllers/SysbotBase/SysbotBase_ProController.cpp

@@ -256,6 +256,7 @@ void ProController_SysbotBase::send_diff(
     ){
         double fx = JoystickTools::linear_u8_to_float(new_state.left_x);
         double fy = -JoystickTools::linear_u8_to_float(new_state.left_y);
+        JoystickTools::clip_magnitude(fx, fy);
 //        cout << "fx = " << fx << ", fy = " << fy << endl;
         int16_t ix = JoystickTools::linear_float_to_s16(fx);
         int16_t iy = JoystickTools::linear_float_to_s16(fy);
@@ -271,6 +272,7 @@ void ProController_SysbotBase::send_diff(
     ){
         double fx = JoystickTools::linear_u8_to_float(new_state.right_x);
         double fy = -JoystickTools::linear_u8_to_float(new_state.right_y);
+        JoystickTools::clip_magnitude(fx, fy);
         int16_t ix = JoystickTools::linear_float_to_s16(fx);
         int16_t iy = JoystickTools::linear_float_to_s16(fy);
         message += "setStick RIGHT ";