[Przemek 4x4]

Witam. Potrzebuję pomocy w ogarnięciu tematu sterowania kontrolerem xbox poprzez esp32s3 6-ciu esc poprzez PWM. Zastosowałem bibliotekę bluepad32 oraz Servo. Z połączeniem się kontrolerem z ESP nie mam problemu. Łączy się przez bluetooth. Nie potrafię wysterowac osobno wszystkich 6-ciu wyjść. Chciał bym aby lewy drążek w górę sterował 4 i 5 wyjściem. Drążek w lewo wyjście 4. Drążek w prawo wyjście 5. Drążek w dół wyjście 6 i 7. R2 wyjście 15. L2 wyjście 16. Załączam program:

#include <Bluepad32.h>
#include <ESP32Servo.h>
// Definicja pinów dla ESC (ESP32-S3 GPIO)
#define ESC1_PIN 4
#define ESC2_PIN 5
#define ESC3_PIN 6
#define ESC4_PIN 7
#define ESC5_PIN 15 // R2
#define ESC6_PIN 16 // L2
Servo esc1, esc2, esc3, esc4, esc5, esc6;
ControllerPtr myControllers[BP32_MAX_GAMEPADS];
void setup() {
    Serial.begin(115200);
   
    // Inicjalizacja i Uzbrajanie ESC (Arming)
    esc1.attach(ESC1_PIN, 1000, 2000);
    esc2.attach(ESC2_PIN, 1000, 2000);
    esc3.attach(ESC3_PIN, 1000, 2000);
    esc4.attach(ESC4_PIN, 1000, 2000);
    esc5.attach(ESC5_PIN, 1000, 2000);
    esc6.attach(ESC6_PIN, 1000, 2000);
    Serial.println("Uzbrajanie ESC... Czekaj 2s");
    armESC();
    // Konfiguracja Bluepad32
    BP32.setup(&onConnectedController, &onDisconnectedController);
}
void armESC() {
    esc1.writeMicroseconds(1000);
    esc2.writeMicroseconds(1000);
    esc3.writeMicroseconds(1000);
    esc4.writeMicroseconds(1000);
    esc5.writeMicroseconds(1000);
    esc6.writeMicroseconds(1000);
    delay(2000); // Czas na inicjalizację kontrolera ESC
    Serial.println("Gotowy!");
}
void processGamepad(ControllerPtr ctl) {
    // Odczyt drążków (-511 do 512) i triggerów (0 do 1023)
    int lx = ctl->axisX();
    int ly = ctl->axisY();
    int r2 = ctl->throttle(); // R2/RT
    int l2 = ctl->brake();    // L2/LT
    // Mapowanie 1-4 (Drążek Lewy)
    // Bluepad32: góra/lewo to wartości ujemne
    int speed = map(max(abs(lx), abs(ly)), 0, 512, 1000, 2000);
    // ESC 1: Lewo + Góra
    esc1.writeMicroseconds((lx < -100 && ly < -100) ? speed : 1000);
    // ESC 2: Prawo + Góra (Poprawka: W opisie było 2x lewo+góra, przyjąłem logiczny rozkład)
    esc2.writeMicroseconds((lx > 100 && ly < -100) ? speed : 1000);
    // ESC 3: Lewo + Dół
    esc3.writeMicroseconds((lx < -100 && ly > 100) ? speed : 1000);
    // ESC 4: Prawo + Dół
    esc4.writeMicroseconds((lx > 100 && ly > 100) ? speed : 1000);
    // ESC 5 i 6 (Triggery)
    esc5.writeMicroseconds(map(r2, 0, 1023, 1000, 2000));
    esc6.writeMicroseconds(map(l2, 0, 1023, 1000, 2000));
}
// Obsługa połączenia (naprawia brak reakcji)
void onConnectedController(ControllerPtr ctl) {
    bool foundEmptySlot = false;
    for (int i = 0; i < BP32_MAX_GAMEPADS; i++) {
        if (myControllers[i] == nullptr) {
            myControllers[i] = ctl;
            foundEmptySlot = true;
            Serial.println("Kontroler Xbox podłączony!");
            break;
        }
    }
}
void onDisconnectedController(ControllerPtr ctl) {
    for (int i = 0; i < BP32_MAX_GAMEPADS; i++) {
        if (myControllers[i] == ctl) {
            myControllers[i] = nullptr;
            Serial.println("Kontroler odłączony!");
            break;
        }
    }
}
void loop() {
    BP32.update();
    for (int i = 0; i < BP32_MAX_GAMEPADS; i++) {
        ControllerPtr myController = myControllers[i];
        if (myController && myController->isConnected()) {
            processGamepad(myController);
        }
    }
    delay(10);
}


Program się kompiluje w wgrywa. Tylko nie wiem czemu działa poprawnie tylko Drążek góra oraz lewo prawo i to na wszystkich 6-ciu wyjściach parami. To jest 4,6,15 razem i 5,7,16 też razem. Proszę o pomoc. Pozdrawiam.