Add Defines and steering trim

ps4Controller/include/motormixer.h

@@ -22,6 +22,7 @@ public:
     void setInputScale(Scaling scale);
     void setOutputScale(float scale);
     void setDeadzone(float deadzone);
+    void setTrimSteering(float trimSteering);
 
     Motormixer() = default;
 
@@ -29,4 +30,5 @@ private:
     Scaling m_inputScale = {0.3f, 1.0f};
     float m_outputScale = 100.0f;
     float m_deadzone = 0.1;
+    float m_trimSteering = 0.0f;
 };

ps4Controller/platformio.ini

@@ -27,7 +27,7 @@ board_build.partitions = min_spiffs.csv
 ; upload_port = esp32-3DD870.lan
 
 
-[env:espressif32-OTA]
+[env:espressif32-OTA-Orange]
 platform = espressif32
 ; board = esp32-c3-devkitm-1
 ; board = wemos_d1_mini32
@@ -47,6 +47,10 @@ lib_deps =
 ; To avoid "undefined reference to `esp_spp_init'" error"
 build_flags =
     -Os ; Optimize for size
+    -D JOY_ADDRESS=\"dc:a2:66:dd:72:c0\"
+    -D LEFT_MOTOR_PIN=15
+    -D RIGHT_MOTOR_PIN=16
+    -D TRIM_STEERING=0.025
     ; -D CONFIG_BT_ENABLED=1
     ; -D CONFIG_BT_CLASSIC_ENABLED=1
 
@@ -62,6 +66,36 @@ upload_protocol = espota
 ; upload_port = esp32c3-8783B4.lan
 upload_port = esp32-3DD870.lan
 
+
+
+[env:espressif32-OTA-BIG]
+platform = espressif32
+; board = wemos_d1_mini32
+board = esp32dev
+framework = arduino
+lib_deps =
+    madhephaestus/ESP32Servo
+    
+    ; espidf implementation
+    https://github.com/saippua/PS4-esp-idf
+
+; build_flags = @${PROJECT_DIR}/big-robot-defines.txt
+build_flags =
+    -Os ; Optimize for size
+    -D JOY_ADDRESS=\"dc:a2:66:dd:72:c0\"
+    -D LEFT_MOTOR_PIN=25
+    -D RIGHT_MOTOR_PIN=32
+
+monitor_speed = 115200
+
+
+board_build.partitions = min_spiffs.csv
+; to save size
+build_type = release
+
+upload_protocol = espota
+upload_port = esp32-3A6900.lan
+
 [env:espressif32-USB]
 platform = espressif32
 ; board = esp32-c3-devkitm-1
@@ -82,6 +116,9 @@ lib_deps =
 ; To avoid "undefined reference to `esp_spp_init'" error"
 build_flags =
     -Os ; Optimize for size
+    -D JOY_ADDRESS=\"dc:a2:66:dd:72:c0\"
+    -D LEFT_MOTOR_PIN=15
+    -D RIGHT_MOTOR_PIN=16
     ; -D CONFIG_BT_ENABLED=1
     ; -D CONFIG_BT_CLASSIC_ENABLED=1
 

ps4Controller/src/OTA.cpp

@@ -2,10 +2,9 @@
 
 #include <ArduinoOTA.h>
 
-void OTA::setup()
 // Initialize OTA
+void OTA::setup()
 {
-    // Initialize OTA
     ArduinoOTA.onStart([]()
                        {
       String type = (ArduinoOTA.getCommand() == U_FLASH) ? "sketch" : "filesystem";
@@ -26,8 +25,8 @@ void OTA::setup()
     ArduinoOTA.begin();
 }
 
-void OTA::loop()
 // Handle OTA updates
+void OTA::loop()
 {
     ArduinoOTA.handle();
 }

ps4Controller/src/controller.cpp

@@ -2,7 +2,7 @@
 #include <PS4Controller.h> // Include the PS4-esp32 library
 
 namespace {
-    #define EXPO 0.7
+    #define EXPO 0.3
     // z = x*(1.0-y)+x^3*y
     float ExpoCurve(float x)
     {

ps4Controller/src/main.cpp

@@ -19,7 +19,9 @@ const char* password = "Johansson85";
 
 OTA ota;
 
-Controller controller{"dc:a2:66:dd:72:c0"};
+// Controller controller{"dc:a2:66:dd:72:c0"};
+Controller controller(JOY_ADDRESS);
+
 // Controller controller;
 Motormixer motormixer;
 
@@ -46,13 +48,18 @@ void setup() {
   ota.setup();
 
   // Attach servos
-  leftMotor.attach(15);
-  rightMotor.attach(16);
+  // leftMotor.attach(15);
+  // rightMotor.attach(16);
+  leftMotor.attach(LEFT_MOTOR_PIN);
+  rightMotor.attach(RIGHT_MOTOR_PIN);
 
   
   // Initialize PS4 controller
   controller.setup();
   motormixer.setOutputScale(100.0);
+  #ifdef TRIM_STEERING
+  motormixer.setTrimSteering(TRIM_STEERING);
+  #endif
   // PS4.begin("7C:9E:BD:3D:D8:72"); // Replace with your ESP32's Bluetooth MAC address
   // PS4.begin("DC:A2:66:DD:72:C0");
   // PS4.begin("dc:a2:66:dd:72:c0");

ps4Controller/src/motormixer.cpp

@@ -17,21 +17,42 @@ void Motormixer::setDeadzone(float deadzone)
     m_deadzone = deadzone;
 }
 
+void Motormixer::setTrimSteering(float trimSteering)
+{
+    m_trimSteering = trimSteering;
+}
+
 Motormixer::Output Motormixer::calculate(float throttle, float steering)
 {
-
-    float steeringMagnitude = fabs(steering);
-    float throttleMagnitude = fabs(throttle);
-
-    // Apply deadzone
-    if (throttleMagnitude < m_deadzone)
+    
+    auto compensateDeadzone = [&](float x) -> float
     {
-        throttle = 0.0;
-    }
-    if (steeringMagnitude < m_deadzone)
-    {
-        steering = 0.0;
-    }
+        const float deadzoneComp = 1.0f - m_deadzone;
+
+        if (fabs(x) < m_deadzone)
+        {
+            return 0.0f;
+        }
+        else
+        {
+            x *= deadzoneComp;
+            if (x > 0.0f)
+                x -= m_deadzone;
+            else
+            {
+                x += m_deadzone;
+            }
+            return x;
+        }
+    };
+
+    auto steeringMagnitude = fabs(steering);
+    auto throttleMagnitude = fabs(throttle);
+
+    throttle = compensateDeadzone(throttle);
+    steering = compensateDeadzone(steering);
+
+    steering += m_trimSteering;
 
     // Apply scaling
     throttle *= m_inputScale.throttle;