Code used for https://youtu.be/lI0AvOpP5sQ

rc_car_v2.ino

#include <Wire.h>
#include <Bluepad32.h>
#include <SensorFusion.h>
#include <Adafruit_NeoPixel.h>
#include <AudioTools.h>
#include <AudioTools/AudioLibs/Concurrency.h>
#include <SparkFun_LSM6DSV16X.h>
#include <ESP32Servo.h>
#include <ESP32PWM.h>
#include <Preferences.h>

#define PIN_STATUS_LED 2
#define PIN_STEERING_SERVO 4
#define PIN_LIGHT_DATA 25
#define PIN_I2S_LRCLK 12
#define PIN_I2S_BITCLK 27
#define PIN_I2S_DATA 33
#define PIN_MOTOR_ENABLE 15
#define PIN_MOTOR_FORWARD 14
#define PIN_MOTOR_BACKWARD 32

#define LED_RIGHT_HEADLIGHT 0
#define LED_LEFT_HEADLIGHT 1
#define LED_LEFT_TAILLIGHT 2
#define LED_RIGHT_TAILLIGHT 3

#define STEERING_CENTERED 1500
#define STEERING_LEFT STEERING_CENTERED - 210
#define STEERING_RIGHT STEERING_CENTERED + 210

ControllerPtr myCtl;
ControllerPtr myTpad;

Adafruit_NeoPixel leds(4, PIN_LIGHT_DATA, NEO_RGB + NEO_KHZ800);
Adafruit_NeoPixel status(1, PIN_STATUS_LED, NEO_GRB + NEO_KHZ800);

AudioInfo snd_info(44100, 1, 16);
SquareWaveGenerator<int16_t> osc;
SquareWaveGenerator<int16_t> osc2;
GeneratorMixer<int16_t> mixer;
GeneratedSoundStream<int16_t> snd(mixer);
AudioEffectStream effects(snd);
I2SStream i2s_out;
ADSRGain adsr(0.001, 0.001, 0.9, 0.0002);
StreamCopy copier(i2s_out, effects);
void audioLoop();
Task audioTask("audioLoop", 10240, 10, 1);

Servo steeringServo;

SparkFun_LSM6DSV16X imu;
bool imu_ok = true;
sfe_lsm_data_t accelData;
// sfe_lsm_data_t gyroData;

SF controllerPose;
// SF carPose;

Preferences preferences;
bool mustRepair = false;

void setup() {
    // set up steering servo
    steeringServo.attach(PIN_STEERING_SERVO);
    steeringServo.writeMicroseconds(STEERING_CENTERED);

    Serial.begin(115200);
    Serial.println("ESP32 Boot");
    Wire.begin();

    preferences.begin("ps5car", false);
    int counter = preferences.getInt("bootCount", 0);
    if (counter > 0) mustRepair = true;
    preferences.putInt("bootCount", counter + 1);

    // set up drive
    pinMode(PIN_MOTOR_ENABLE, OUTPUT);
    pinMode(PIN_MOTOR_FORWARD, OUTPUT);
    pinMode(PIN_MOTOR_BACKWARD, OUTPUT);
    analogWriteFrequency(32000);

    // set up LEDs
    leds.setBrightness(50);
    leds.begin();
    leds.clear();
    leds.show();

    // set up LEDs
    status.setBrightness(50);
    status.begin();
    status.clear();
    status.show();

    // set up IMU
    if (!imu.begin()) {
        Serial.println("init IMU fail");
        imu_ok = false;
    } else {
        imu.deviceReset();
        while (!imu.getDeviceReset()) yield();
        imu.enableBlockDataUpdate();
        imu.setAccelDataRate(LSM6DSV16X_ODR_AT_1920Hz);
        imu.setAccelFullScale(LSM6DSV16X_16g);
        // imu.setGyroDataRate(LSM6DSV16X_ODR_AT_15Hz);
        // imu.setGyroFullScale(LSM6DSV16X_4000dps);
        imu.enableFilterSettling();
        imu.enableAccelLP2Filter();
        imu.setAccelLP2Bandwidth(LSM6DSV16X_XL_VERY_LIGHT);
        // imu.enableGyroLP1Filter();
        // imu.setGyroLP1Bandwidth(LSM6DSV16X_GY_VERY_LIGHT);
    }

    // set up audio
    AudioLogger::instance().begin(Serial, AudioLogger::Error);
    auto config = i2s_out.defaultConfig(TX_MODE);
    config.copyFrom(snd_info);
    config.pin_bck = PIN_I2S_BITCLK;
    config.pin_ws = PIN_I2S_LRCLK;
    config.pin_data = PIN_I2S_DATA;
    mixer.add(osc);
    mixer.add(osc2);
    mixer.begin(config);
    i2s_out.begin(config);
    osc.begin(config, N_E4);
    osc2.begin(config, N_G4);
    effects.begin(config);
    effects.addEffect(adsr);
    audioTask.begin(audioLoop);

    // set up bluetooth
    BP32.setup(&gp_onConnect, &gp_onDisconnect);
    if (mustRepair) BP32.forgetBluetoothKeys();
    BP32.enableVirtualDevice(true);
    Serial.printf("Using Bluepad32: %s\n", BP32.firmwareVersion());
}

void loop() {
    vTaskDelay(10);
    yield();

    // check input from gamepad
    BP32.update();
    checkControllers();
    if (myCtl != NULL && myCtl->isConnected() && myCtl->hasData() && myTpad != NULL && myTpad->isConnected() && myTpad->hasData()) {
        controllerPose.MahonyUpdate(
          myCtl->gyroX() / 1024. * DEG_TO_RAD,
          myCtl->gyroX() / 1024. * DEG_TO_RAD,
          myCtl->gyroZ() / 1024. * DEG_TO_RAD,
          myCtl->accelX(),
          myCtl->accelY(),
          myCtl->accelZ(),
          controllerPose.deltatUpdate());
        // Serial.printf("roll=%10g pitch=%10g yaw=%10g\n", controllerPose.getRoll(), controllerPose.getPitch(), controllerPose.getYaw());

        doDrive(-myCtl->axisY() - myCtl->axisRY() /* up is negative */, myCtl->throttle(), myCtl->brake());
        doSteering(myCtl->axisRX() + myCtl->axisX(), controllerPose.getPitch());
        controlLights(myCtl->dpad(), myTpad->deltaX(), myTpad->deltaY());
        honk(myCtl->a());  // South = cross
    } else {
        controllerPose.MahonyUpdate(0, 0, 0, 0, 0, 0, controllerPose.deltatUpdate());
    }

    // check IMU for crashes
    if (imu_ok && imu.checkAccelStatus()) {
        imu.getAccel(&accelData);
        // imu.getGyro(&gyroData);
        processCrashDetect(&accelData);
    }

    checkBootCountTimeout();
}

int remap_throttle_to_pwm(int throttle) {
    return 8 * sqrt(throttle);  // maps 1024 -> 255, with throttle correction
}

void doDrive(int stickThrottle, int triggerThrottle, int brake) {
#define THROTTLE_DEADZONE 32
    static int throttle = 0;
    static bool isForward = true;
    throttle = max(triggerThrottle, abs(stickThrottle));
    if (triggerThrottle < abs(stickThrottle)) isForward = stickThrottle > 0;
    if (throttle < THROTTLE_DEADZONE) {
        throttle = 0;
        isForward = true;
    }
    int totalThrottle = throttle - brake;

    if (totalThrottle > THROTTLE_DEADZONE) {
        analogWrite(PIN_MOTOR_ENABLE, 255);
        if (isForward) {
            analogWrite(PIN_MOTOR_BACKWARD, 0);
            analogWrite(PIN_MOTOR_FORWARD, remap_throttle_to_pwm(throttle));
        } else {
            analogWrite(PIN_MOTOR_FORWARD, 0);
            analogWrite(PIN_MOTOR_BACKWARD, remap_throttle_to_pwm(throttle));
        }
    } else if (totalThrottle < -THROTTLE_DEADZONE) {
        // handle brake
        analogWrite(PIN_MOTOR_FORWARD, 0);
        analogWrite(PIN_MOTOR_BACKWARD, 0);
        analogWrite(PIN_MOTOR_ENABLE, remap_throttle_to_pwm(-brake));
    } else {
        analogWrite(PIN_MOTOR_FORWARD, 0);
        analogWrite(PIN_MOTOR_BACKWARD, 0);
        analogWrite(PIN_MOTOR_ENABLE, 0);
    }
}

void doSteering(int steering, float pitch) {
    int realSteering = constrain(steering + (pitch * abs(pitch)) / 8, -512, 512);
    int servoPulse = abs(realSteering) < 16 ? STEERING_CENTERED : map(realSteering, -512, 512, STEERING_LEFT, STEERING_RIGHT);
    steeringServo.writeMicroseconds(servoPulse);
}

void controlLights(int dpad, int dx, int dy) {
    static int lastRequest = 0;

    static bool headlightsOn = false;
    static bool reverseLightsOn = false;
    enum Blinker : int { B_OFF = 0,
                         B_LEFT = 1,
                         B_RIGHT = 2 };
    static enum Blinker blinkers = B_OFF;
    static int lightHue = 65;
    static int lightSaturation = 0;

    if (dpad != lastRequest) {
        lastRequest = dpad;
        if (dpad & DPAD_UP) headlightsOn = !headlightsOn;
        if (dpad & DPAD_DOWN) reverseLightsOn = !reverseLightsOn;
        if (dpad & DPAD_LEFT) blinkers = blinkers == B_LEFT ? B_OFF : B_LEFT;
        if (dpad & DPAD_RIGHT) blinkers = blinkers == B_RIGHT ? B_OFF : B_RIGHT;
    }
    lightHue = (lightHue + dx / 3 + 360) % 360;
    lightSaturation = constrain(lightSaturation + dy, 0, 100);

    static int lastHeadlightColor = 0;
    int headlightColor = hsv2rgb(lightHue, lightSaturation, 100);
    int headlight = headlightsOn ? headlightColor : 0;
    int taillight = reverseLightsOn ? headlightColor : (headlightsOn ? 0x440000 : 0);
    bool blinkOn = (millis() % 700) > 350;

    leds.setPixelColor(LED_LEFT_HEADLIGHT, (blinkers & B_LEFT) && blinkOn ? 0xFF7700 : headlight);
    leds.setPixelColor(LED_RIGHT_HEADLIGHT, (blinkers & B_RIGHT) && blinkOn ? 0xFF7700 : headlight);
    leds.setPixelColor(LED_LEFT_TAILLIGHT, (blinkers & B_LEFT) && blinkOn ? 0xFF0000 : taillight);
    leds.setPixelColor(LED_RIGHT_TAILLIGHT, (blinkers & B_RIGHT) && blinkOn ? 0xFF0000 : taillight);
    leds.show();
    if (myCtl != NULL && myCtl->isConnected() && lastHeadlightColor != headlightColor) {
        myCtl->setColorLED((headlightColor >> 16) & 255, (headlightColor >> 8) & 255, headlightColor & 255);
        lastHeadlightColor = headlightColor;
    }
}

int hsv2rgb(int H, int S, int V) {
    // modified from https://github.com/Inseckto/HSV-to-RGB/blob/master/HSV2RGB.c
    float r, g, b;

    float h = H / 360.;
    float s = S / 100.;
    float v = V / 100.;

    int i = floor(h * 6);
    float f = h * 6 - i;
    float p = v * (1 - s);
    float q = v * (1 - f * s);
    float t = v * (1 - (1 - f) * s);

    switch (i % 6) {
        case 0: r = v, g = t, b = p; break;
        case 1: r = q, g = v, b = p; break;
        case 2: r = p, g = v, b = t; break;
        case 3: r = p, g = q, b = v; break;
        case 4: r = t, g = p, b = v; break;
        case 5: r = v, g = p, b = q; break;
    }

    int ri = r * 255, gi = g * 255, bi = b * 255;
    return (ri << 16) | (gi << 8) | bi;
}

void honk(bool isOn) {
    static bool wasOn = false;
    if (isOn && !wasOn) {
        adsr.keyOn();
    }
    if (!isOn && wasOn) {
        adsr.keyOff();
    }
    wasOn = isOn;
}

void processCrashDetect(sfe_lsm_data_t* accelData) {
    if (abs(accelData->zData) > 1000) {  // z is facing forwards/backwards
        if (myCtl != NULL) {
            myCtl->playDualRumble(0, 500, 255, 255);
        }
    }
}

void audioLoop() {
    copier.copy();
    yield();
}

ControllerPtr controllers[BP32_MAX_CONTROLLERS];

void checkControllers() {
    if (myCtl != NULL && myTpad != NULL) {
        status.setPixelColor(0, 0x00FF00);
        status.show();
        // if it disconnects later, we aren't gonna forget the keys, so it doesn't need a re-pair.
        mustRepair = false;
        return;
    }
    int ptime = map(millis() % 700, 0, 700, 0, 512);
    int bri = 0;
    if (mustRepair) bri = ptime < 64 || (ptime >= 128 && ptime < 192) ? 255 : 0;
    else bri = ptime > 256 ? 512 - ptime : ptime;
    status.setPixelColor(0, bri & 255);
    status.show();

    for (int i = 0; i < BP32_MAX_CONTROLLERS; i++) {
        ControllerPtr ctl = controllers[i];
        if (ctl == NULL) continue;
        if (ctl->isGamepad()) {
            myCtl = ctl;
            Serial.println("Got gpad");
        }
        if (ctl->isMouse()) {
            myTpad = ctl;
            Serial.println("Got mous");
        }
    }
}

void gp_onConnect(ControllerPtr ctl) {
    Serial.printf("Controller %s connected\n", ctl->getModelName().c_str());
    for (int i = 0; i < BP32_MAX_CONTROLLERS; i++) {
        if (controllers[i] == NULL) {
            controllers[i] = ctl;
            return;
        }
    }
}

void gp_onDisconnect(ControllerPtr ctl) {
    Serial.printf("Controller %s disconnected\n", ctl->getModelName().c_str());
    if (myCtl == ctl) {
        Serial.println("Lost gpad");
        myCtl = NULL;
    }
    if (myTpad == ctl) {
        Serial.println("Lost mous");
        myTpad = NULL;
    }
    for (int i = 0; i < BP32_MAX_CONTROLLERS; i++) {
        if (controllers[i] == ctl) {
            controllers[i] = NULL;
            return;
        }
    }
}

void checkBootCountTimeout() {
    static bool timeouted = false;
    if (!timeouted && millis() > 5000) {
        preferences.putInt("bootCount", 0);
        timeouted = true;
    }
}