Race Game DIY

				
					#include <Bluepad32.h>
#include <ESP32Servo.h>

const int servoPin = 12; // yellow wire
const int escPin = 13; // white wire
Servo servo1;
Servo esc;

// ================= LED VALUES =========================
bool copLightsActive, lightActive = false; // LED state
bool escAlreadySet = false;
bool previousButtonState = HIGH; // Previous button state

const int ledBrakePin = 14;
const int ledHeadlightPin = 27;

// ================= PEDALS VALUES =========================
int throttleValue, brakeValue, escValue, servoValue, panValue, tiltValue, gyroValue, accelValue;
int escSlowSpeed = 200; // int escSlowSpeed = 1700;
int escNormalSpeed = 300; // int escNormalSpeed = 1800;
int escMaxSpeed = 500; // int escMaxSpeed = 2000;

// ================= NITRO VALUES =========================
int nitroAmmo = 1;
int nitroDuration = 5000; // 5 seconds
unsigned long nitroStartTime = 0;
bool nitroActivated = false; // 1==false ; 0==true;


// ================= GUN VALUES =========================
int gunAmmo = 1;
int gunDuration = 3000; // 3 seconds
unsigned long gunStartTime = 0;
bool gunActivated = false;

// ================= HORN VALUES =========================
int hornAmmo = 1;
int hornDuration = 3000; // 3 seconds
unsigned long hornStartTime = 0;
bool hornActivated = false;


// ================= BUTTON PRESSED VALUES =========================
// Define global variables
float value = 100.0; // Initial value (100%)
unsigned long lastSubtractTime = 0; // Time tracker for subtracting
unsigned long lastAddTime = 0;      // Time tracker for adding
unsigned long buttonReleaseTime = 0;// Time when the button was released
bool buttonHeld = false;            // Whether the button is held
bool buttonReleased = false;        // Whether the button was released
bool addingActive = false;       

// Array of values
int values[] = {10, 20, 30, 40, 50};
int arrayLength = sizeof(values) / sizeof(values[0]);  // Calculate the array length
int currentIndex = 0;  // Start at the first value in the array


ControllerPtr myControllers[BP32_MAX_GAMEPADS];

// This callback gets called any time a new gamepad is connected.
// Up to 4 gamepads can be connected at the same time.
void onConnectedController(ControllerPtr ctl) {
  bool foundEmptySlot = false;
  for (int i = 0; i < BP32_MAX_GAMEPADS; i++) {
    if (myControllers[i] == nullptr) {
      Serial.printf("CALLBACK: Controller is connected, index=%d\n", i);
      // Additionally, you can get certain gamepad properties like:
      // Model, VID, PID, BTAddr, flags, etc.
      ControllerProperties properties = ctl->getProperties();
      Serial.printf("Controller model: %s, VID=0x%04x, PID=0x%04x\n", ctl->getModelName().c_str(), properties.vendor_id, properties.product_id);
      myControllers[i] = ctl;
      foundEmptySlot = true;
      break;
      }
    }

    if (!foundEmptySlot) {
      Serial.println("CALLBACK: Controller connected, but could not found empty slot");
    }
}

void onDisconnectedController(ControllerPtr ctl) {
  bool foundController = false;

  for (int i = 0; i < BP32_MAX_GAMEPADS; i++) {
    if (myControllers[i] == ctl) {
      Serial.printf("CALLBACK: Controller disconnected from index=%d\n", i);
      myControllers[i] = nullptr;
      foundController = true;
      break;
    }
  }

    if (!foundController) {
      Serial.println("CALLBACK: Controller disconnected, but not found in myControllers");
    }
}

// ========= SEE CONTROLLER VALUES IN SERIAL MONITOR ========= //

void dumpGamepad(ControllerPtr ctl) {
  Serial.printf(
  "idx=%d, dpad: 0x%02x, buttons: 0x%04x, axis L: %4d, %4d, axis R: %4d, %4d, brake: %4d, throttle: %4d, "
  "misc: 0x%02x, gyro x:%6d y:%6d z:%6d, accel x:%6d y:%6d z:%6d\n",
  ctl->index(),        // Controller Index
  ctl->dpad(),         // D-pad
  ctl->buttons(),      // bitmask of pressed buttons
  ctl->axisX(),        // (-511 - 512) left X Axis
  ctl->axisY(),        // (-511 - 512) left Y axis
  ctl->axisRX(),       // (-511 - 512) right X axis
  ctl->axisRY(),       // (-511 - 512) right Y axis
  ctl->brake(),        // (0 - 1023): brake button
  ctl->throttle(),     // (0 - 1023): throttle (AKA gas) button
  ctl->miscButtons(),  // bitmask of pressed "misc" buttons
  ctl->gyroX(),        // Gyro X
  ctl->gyroY(),        // Gyro Y
  ctl->gyroZ(),        // Gyro Z
  ctl->accelX(),       // Accelerometer X
  ctl->accelY(),       // Accelerometer Y
  ctl->accelZ()        // Accelerometer Z
  );
}

// ========= GAME CONTROLLER ACTIONS SECTION ========= //

void processGamepad(ControllerPtr ctl) {
  // There are different ways to query whether a button is pressed.
  // By query each button individually:
  //  a(), b(), x(), y(), l1(), etc...
 
  //== PS4 X button = 0x0001 ==//
  if (ctl->buttons() == 0x0001) {
    // code for when X button is pushed
    checkNitroButton();

  }
  if (ctl->buttons() != 0x0001) {
    // code for when X button is released
  }

  //== PS4 Square button = 0x0004 ==//
  if (ctl->buttons() == 0x0004) {
    // code for when square button is pushed
    checkGunButton();
  }
  if (ctl->buttons() != 0x0004) {
  // code for when square button is released
  }

  //== PS4 Triangle button = 0x0008 ==//
  if (ctl->buttons() == 0x0008) {
    checkHornButton();
    // code for when triangle button is pushed
  }
  if (ctl->buttons() != 0x0008) {
    // code for when triangle button is released
  }

  //== PS4 Circle button = 0x0002 ==//
  if (ctl->buttons() == 0x0002) {
    // code for when circle button is pushed
    checkHornButton();
  }
  if (ctl->buttons() != 0x0002) {
    // code for when circle button is released
  }
  
  //== PS4 Share miscButtons = 0x02 ==//
  if (ctl->miscButtons() == 0x02) {
    // code for when Share button is pushed
      Serial.print("Share button is pushed ");
  }
  //== PS4 Option miscButtons = 0x04 ==//
  if (ctl->miscButtons() == 0x04) {
    // code for when Option button is pushed
      Serial.print("Option button is pushed ");
  }
  //== PS4 PS miscButtons = 0x01 ==//
  if (ctl->miscButtons() == 0x01) {
    // code for when PS button is pushed
      Serial.print("PS button is pushed ");
  }

  //== PS4 Dpad UP button = 0x01 ==//
  if (ctl->dpad() == 0x01) {
    // code for when dpad up button is pushed
    escValue = 2000;
  }
  if (ctl->dpad() != 0x01) {
    // code for when dpad up button is released
    escValue = 1500;
  }

  //==PS4 Dpad DOWN button = 0x02==//
  if (ctl->dpad() == 0x02) {
    // code for when dpad down button is pushed
    escValue = 1000;
  }
  if (ctl->dpad() != 0x02) {
    // code for when dpad down button is released
    escValue = 1500;
  }

  //== PS4 Dpad LEFT button = 0x08 ==//
  if (ctl->dpad() == 0x08) {
    // code for when dpad left button is pushed
    servoValue = 0;
  }
  if (ctl->dpad() != 0x08) {
    // code for when dpad left button is released
    servoValue = 90;
  }

  //== PS4 Dpad RIGHT button = 0x04 ==//
  if (ctl->dpad() == 0x04) {
    // code for when dpad right button is pushed
    servoValue = 180;
  }
  if (ctl->dpad() != 0x04) {
    // code for when dpad right button is released
    servoValue = 90;
  }

  //== PS4 R1 trigger button = 0x0020 ==//
  if (ctl->buttons() == 0x0020) {
    // code for when R1 button is pushed
  }
  if (ctl->buttons() != 0x0020) {
    // code for when R1 button is released
  }

  //== PS4 R2 trigger button = 0x0080 ==//
  if (ctl->throttle() >= 25) {
    // code for when R2 button is pushed
    if (nitroActivated == true){
        throttleValue = map(ctl->throttle(), 0, 1023, 0, escMaxSpeed);
    }else{
        throttleValue = map(ctl->throttle(), 0, 1023, 0, escNormalSpeed);
    }
      Serial.print("throttleValue: ");
      Serial.println(throttleValue);
  }
  if (ctl->throttle() < 25) {
    // code for when R2 button is released
    throttleValue = 0;  
  }

  //== PS4 L1 trigger button = 0x0010 ==//
  if (ctl->buttons() == 0x0010) {
    // code for when L1 button is pushed
    toggleLED();
  }
  if (ctl->buttons() != 0x0010) {
    // code for when L1 button is released
  }

  //== PS4 L2 trigger button = 0x0040 ==//
  if (ctl->brake() >= 25) {
    // code for when L2 button is pushed
      brakeValue = map(ctl->brake(), 0, 1023, 0, 500);
      Serial.print("brakeValue: ");
      Serial.println(brakeValue);
  }
  if (ctl->brake() < 25) {
    // code for when L2 button is released
      brakeValue = 0;
  }

  //== LEFT JOYSTICK - UP ==//
  if (ctl->axisY() <= -25) {
    // code for when left joystick is pushed up
    }

  //== LEFT JOYSTICK - DOWN ==//
  if (ctl->axisY() >= 25) {
    // code for when left joystick is pushed down
  }

  //== LEFT JOYSTICK - LEFT/RIGHT ==//
  if (ctl->axisX()) {
    // code for when left joystick is pushed left
      servoValue = map(ctl->axisX(), -512, 512, 0, 180);
      Serial.print("servoValue: ");
      Serial.println(servoValue);
  }
  //== LEFT JOYSTICK - LEFT ==//
  if (ctl->axisX() <= -25) {
    // code for when left joystick is pushed left
  }

  //== LEFT JOYSTICK - RIGHT ==//
  if (ctl->axisX() >= 25) {
    // code for when left joystick is pushed right
  }

  //== LEFT JOYSTICK - LEFT ==//
  if (ctl->gyroY()) {
    // code for when left joystick is pushed left
  
      gyroValue = map(ctl->gyroY(), -512, 512, 0, 180);
      Serial.print("gyroValue: ");
      Serial.println(gyroValue);
  }

  if (ctl->accelY()) {
    // code for when left joystick is pushed left
      accelValue = map(ctl->accelY(), -512, 512, 0, 180);
      Serial.print("accelValue: ");
      Serial.println(accelValue);
  }


  //== LEFT JOYSTICK DEADZONE ==//
  if (ctl->axisY() > -25 && ctl->axisY() < 25 && ctl->axisX() > -25 && ctl->axisX() < 25) {
    // code for when left joystick is at idle
    // escValue = 1500;
  }

  //== RIGHT JOYSTICK - X AXIS ==//
  if (ctl->axisRX()) {
    // code for when right joystick moves along x-axis
    
      panValue = map(ctl->axisRX(), -512, 512, 0, 180);
      // Serial.print("panValue: ");
      // Serial.println(panValue);
  }

  //== RIGHT JOYSTICK - Y AXIS ==//
  if (ctl->axisRY()) {
  // code for when right joystick moves along y-axis
      tiltValue = map(ctl->axisRY(), -512, 512, 0, 180);
      // Serial.print("tiltValue: ");
      // Serial.println(tiltValue);
  }
  //== RIGHT JOYSTICK DEADZONE ==//
  if (ctl->axisRY() > -25 && ctl->axisRY() < 25 && ctl->axisRX() > -25 && ctl->axisRX() < 25) {
    // code for when left joystick is at idle
  }
  
    // Mix both pedal values before sending to the ESC and make sure it's in range
    escValue = constrain((1500 + throttleValue - brakeValue), 1000, 2000);
    // if (escValue >= 1450 && escValue <= 1550 ){ escValue = 1500; }  //make it smoother
    
    esc.writeMicroseconds(escValue);
    servo1.write(servoValue);
    // if the car is going backwards, light up the tail lights
    if (escValue < 1450) {
      digitalWrite(ledBrakePin, HIGH); 
    }else{
      digitalWrite(ledBrakePin, LOW);
    }

  dumpGamepad(ctl);
}

void processControllers() {
  for (auto myController : myControllers) {
    if (myController && myController->isConnected() && myController->hasData()) {
      if (myController->isGamepad()) {
         processGamepad(myController);
      }
      else {
        Serial.println("Unsupported controller");
      }
    }
  }
}

// Arduino setup function. Runs in CPU 1
void setup() {
  Serial.begin(115200);

  servo1.attach(servoPin);
  esc.attach(escPin);
  
  pinMode(ledHeadlightPin, OUTPUT);
  pinMode(ledBrakePin, OUTPUT);

  digitalWrite(ledBrakePin, LOW);
  digitalWrite(ledHeadlightPin, LOW);
  
  Serial.printf("Firmware: %s\n", BP32.firmwareVersion());
  const uint8_t* addr = BP32.localBdAddress();
  Serial.printf("BD Addr: %2X:%2X:%2X:%2X:%2X:%2X\n", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);

  // Setup the Bluepad32 callbacks
  BP32.setup(&onConnectedController, &onDisconnectedController);

  // "forgetBluetoothKeys()" should be called when the user performs
  // a "device factory reset", or similar.
  // Calling "forgetBluetoothKeys" in setup() just as an example.
  // Forgetting Bluetooth keys prevents "paired" gamepads to reconnect.
  // But it might also fix some connection / re-connection issues.
  BP32.forgetBluetoothKeys();

  // Enables mouse / touchpad support for gamepads that support them.
  // When enabled, controllers like DualSense and DualShock4 generate two connected devices:
  // - First one: the gamepad
  // - Second one, which is a "virtual device", is a mouse.
  // By default, it is disabled.
  BP32.enableVirtualDevice(false);
}

// Arduino loop function. Runs in CPU 1.
void loop() {
  // This call fetches all the controllers' data.
  // Call this function in your main loop.
  bool dataUpdated = BP32.update();
  if (dataUpdated)
    processControllers();

    // The main loop must have some kind of "yield to lower priority task" event.
    // Otherwise, the watchdog will get triggered.
    // If your main loop doesn't have one, just add a simple `vTaskDelay(1)`.
    // Detailed info here:
    // https://stackoverflow.com/questions/66278271/task-watchdog-got-triggered-the-tasks-did-not-reset-the-watchdog-in-time

    vTaskDelay(1);
  // delay(150);
}


void setupESC() {
      Serial.print("setupESC triggered ");
  // this is so this code run only once.
  if (escAlreadySet == false) {

    esc.writeMicroseconds(1500);  delay(500); 
    esc.writeMicroseconds(2000);  delay(1000);

    esc.writeMicroseconds(1500);  delay(500);
    esc.writeMicroseconds(1000);  delay(1000);

    esc.writeMicroseconds(1500);  delay(500);  
    esc.writeMicroseconds(1000);  delay(1000);

    esc.writeMicroseconds(1500);

    escAlreadySet = true;
  }
};


void toggleLED() {
      Serial.print("toggleLED triggered ");
    // Read the current button state
    bool currentButtonState = digitalRead(ledHeadlightPin);;

    // Check if the button is pressed and released
    if (previousButtonState == HIGH && currentButtonState == LOW) {
        // Toggle the LED state
        lightActive = !lightActive;

        // Set the LED to the new state
        digitalWrite(ledHeadlightPin, lightActive ? HIGH : LOW);
    }

    // Update the previous button state for the next loop
    previousButtonState = currentButtonState;
}


void checkNitroButton() {
      Serial.print("checkNitroButton triggered ");
    if (!nitroActivated) {
        // Button pressed and nitro is not currently in use
        nitroStartTime = millis(); // Record the time when nitro was activated
        nitroActivated = true;
        nitroAmmo--;
    }

    // Check if X seconds have passed since nitro was activated
    if (nitroActivated && (millis() - nitroStartTime >= nitroDuration)) {
        nitroActivated = false;
    }
};

void checkGunButton() {
      Serial.print("checkGunButton triggered ");
    if (!gunActivated) {
        // Button pressed and gun is not currently in use
        gunStartTime = millis(); // Record the time when gun was activated
        gunActivated = true;
        gunAmmo--;
    }

    // Check if X seconds have passed since gun was activated
    if (gunActivated && (millis() - gunStartTime >= gunDuration)) {
        gunActivated = false;
    }
};

void checkHornButton() {
      Serial.print("checkHornButton triggered ");
    if (!hornActivated) {
        // Button pressed and nitro is not currently in use
        hornStartTime = millis(); // Record the time when nitro was activated
        hornActivated = true;
        hornAmmo--;
    }

    // Check if X seconds have passed since nitro was activated
    if (hornActivated && (millis() - hornStartTime >= hornDuration)) {
        hornActivated = false;
    }
};