Last active
March 7, 2025 20:01
-
-
Save mbebjak/70fa2d25055845298bac3fdf75158f0d to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #include <Bluepad32.h> | |
| const uint cakanieMedziCyklami = 10; //ms | |
| const uint minimalnaSila = 20; | |
| struct Motor { | |
| String meno; | |
| uint8_t pin1; | |
| uint8_t pin2; | |
| }; | |
| struct KoncoveSpinace { | |
| uint8_t pin1; | |
| uint8_t pin2; | |
| }; | |
| struct LED { | |
| String meno; | |
| uint8_t pin; | |
| }; | |
| const Motor motorNavijak = {"navjak", 32, 33}; | |
| const Motor motorVysuvRamena = {"vysuv ramena", 26, 25}; | |
| const KoncoveSpinace koncakVysuvRamena = {12, 15}; | |
| const Motor motorZdvihRamena = {"zdvih ramena", 21, 13}; | |
| const KoncoveSpinace koncakZdvihRamena = {22, 23}; | |
| const Motor motorPodpery = {"podpery", 19, 18}; | |
| const KoncoveSpinace koncakPodpery = {34, 35}; | |
| const Motor motorOtacanieVeze = {"otacanie veze", 16, 17}; | |
| const LED ledRameno = {"rameno", 14}; | |
| const LED ledKabina = {"kabina", 4}; | |
| const uint8_t pinTruba = 27; | |
| void nastavPiny() { | |
| nastavPinyPreMotor(motorNavijak); | |
| nastavPinyPreMotor(motorVysuvRamena); | |
| nastavPinyPreKoncaky(koncakVysuvRamena); | |
| nastavPinyPreMotor(motorZdvihRamena); | |
| nastavPinyPreKoncaky(koncakZdvihRamena); | |
| nastavPinyPreMotor(motorPodpery); | |
| nastavPinyPreKoncaky(koncakPodpery); | |
| nastavPinyPreMotor(motorOtacanieVeze); | |
| pinMode(ledRameno.pin, OUTPUT); | |
| pinMode(ledKabina.pin, OUTPUT); | |
| pinMode(pinTruba, OUTPUT); | |
| // meni PWM frekvenciu pre vsetky piny. moze byt problem s motormi | |
| analogWriteFrequency(270); | |
| } | |
| void nastavPinyPreMotor(Motor motor) { | |
| pinMode(motor.pin1, OUTPUT); | |
| pinMode(motor.pin2, OUTPUT); | |
| } | |
| void nastavPinyPreKoncaky(KoncoveSpinace koncaky) { | |
| pinMode(koncaky.pin1, INPUT_PULLUP); | |
| pinMode(koncaky.pin2, INPUT_PULLUP); | |
| } | |
| void ovladaj(ControllerPtr ctl) { | |
| //Serial.printf("x: %d, y: %d, a: %d, b: %d\n", ctl->x(), ctl->y(), ctl->a(), ctl->b()); | |
| //dumpGamepad(ctl); | |
| //return; | |
| int silaVysuvuRamena = | |
| ovladajMotorsKoncakmi(motorVysuvRamena, hodnotaPacky(ctl->axisY()), koncakVysuvRamena); | |
| ovladajNavijak(motorNavijak, hodnotaPacky(ctl->axisRY()), silaVysuvuRamena); | |
| ovladajMotorsKoncakmi(motorZdvihRamena, hodnotaPacky(ctl->axisX()), koncakZdvihRamena); | |
| ovladajMotor(motorOtacanieVeze, hodnotaPacky(ctl->axisRX())); | |
| ovladajMotorsKoncakmi(motorPodpery, hodnotaVysuvuPodpier(ctl), koncakPodpery); | |
| ovladajTrubu(ctl); | |
| ovladajSvetla(ctl); | |
| } | |
| void ovladajTrubu(ControllerPtr ctl) { | |
| if (ctl->x()) { | |
| Serial.printf("Trubim "); | |
| analogWrite(pinTruba, 100); | |
| return; | |
| } | |
| analogWrite(pinTruba, 0); | |
| } | |
| void ovladajSvetla(ControllerPtr ctl) { | |
| if (ctl->dpad() == DPAD_UP) { | |
| zapniSvetlo(ledRameno); | |
| } | |
| if (ctl->dpad() == DPAD_DOWN) { | |
| vypniSvetlo(ledRameno); | |
| } | |
| if (ctl->dpad() == DPAD_RIGHT) { | |
| zapniSvetlo(ledKabina); | |
| } | |
| if (ctl->dpad() == DPAD_LEFT) { | |
| vypniSvetlo(ledKabina); | |
| } | |
| } | |
| void zapniSvetlo(LED led) { | |
| Serial.printf("Zapinam svetlo %s\n", led.meno); | |
| analogWrite(led.pin, 255); | |
| } | |
| void vypniSvetlo(LED led) { | |
| Serial.printf("Vypinam svetlo %s\n", led.meno); | |
| analogWrite(led.pin, 0); | |
| } | |
| void ovladajNavijak(Motor motor, int sila, int silaVysuvuRamena) { | |
| ovladajMotor(motor, sila - silaVysuvuRamena * 0.18); | |
| } | |
| int hodnotaVysuvuPodpier(ControllerPtr ctl) { | |
| if (ctl->y()) { | |
| return 250; | |
| } | |
| if (ctl->a()) { | |
| return -250; | |
| } | |
| return 0; | |
| } | |
| /* | |
| * sila <-255, 255> | |
| * vrati skutocnu silu ktorou sa toci motor | |
| */ | |
| int ovladajMotor(Motor motor, int sila) { | |
| if (abs(sila) < minimalnaSila) { | |
| zastavMotor(motor); | |
| return 0; | |
| } | |
| if (sila > 0) { | |
| spustiMotorDoprava(motor, sila); | |
| } else { | |
| spustiMotorDolava(motor, sila); | |
| } | |
| return sila; | |
| } | |
| /* | |
| * sila <-255, 255> | |
| * vrati skutocnu silu ktorou sa toci motor | |
| */ | |
| int ovladajMotorsKoncakmi(Motor motor, int sila, KoncoveSpinace koncoveSpinace) { | |
| if (abs(sila) < minimalnaSila) { | |
| zastavMotor(motor); | |
| return 0; | |
| } | |
| if (sila > 0) { | |
| if (jeKoncakZopnuty(koncoveSpinace.pin1)) { | |
| Serial.printf("Motor %s na koncaku 1\n", motor.meno); | |
| zastavMotor(motor); | |
| return 0; | |
| } | |
| spustiMotorDoprava(motor, sila); | |
| } else { | |
| if (jeKoncakZopnuty(koncoveSpinace.pin2)) { | |
| Serial.printf("Motor %s na koncaku 2\n", motor.meno); | |
| zastavMotor(motor); | |
| return 0; | |
| } | |
| spustiMotorDolava(motor, sila); | |
| } | |
| return sila; | |
| } | |
| void zastavMotor(Motor motor) { | |
| //Serial.printf("Motor %s zastaveny\n", motor.meno); | |
| analogWrite(motor.pin1, 0); | |
| analogWrite(motor.pin2, 0); | |
| } | |
| void spustiMotorDoprava(Motor motor, int sila) { | |
| Serial.printf("Motor %s spusteny silou %d doprava\n", motor.meno, sila); | |
| analogWrite(motor.pin1, min(abs(sila), 255)); | |
| analogWrite(motor.pin2, 0); | |
| } | |
| void spustiMotorDolava(Motor motor, int sila) { | |
| Serial.printf("Motor %s spusteny silou %d dolava\n", motor.meno, sila); | |
| analogWrite(motor.pin1, 0); | |
| analogWrite(motor.pin2, min(abs(sila), 255)); | |
| } | |
| int hodnotaPacky(int hodnotaOvladaca) { | |
| return map(hodnotaOvladaca, -511, 512, -255, 255); | |
| } | |
| bool jeKoncakZopnuty(uint8_t koncakPin) { | |
| return digitalRead(koncakPin) == HIGH; | |
| } | |
| void ovladacPripojeny() { | |
| analogWrite(pinTruba, 50); | |
| delay(100); | |
| analogWrite(pinTruba, 0); | |
| } | |
| // Dalej sa riesi pripojenie gamepadu - TO nemusis pozerat | |
| 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; | |
| ovladacPripojeny(); | |
| 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"); | |
| } | |
| } | |
| 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 | |
| ); | |
| } | |
| void processControllers() { | |
| for (auto myController : myControllers) { | |
| if (myController && myController->isConnected() && myController->hasData()) { | |
| if (myController->isGamepad()) { | |
| ovladaj(myController); | |
| } else { | |
| Serial.println("Unsupported controller"); | |
| } | |
| } | |
| } | |
| } | |
| // Arduino setup function. Runs in CPU 1 | |
| void setup() { | |
| Serial.begin(9600); | |
| 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); | |
| nastavPiny(); | |
| } | |
| // 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(cakanieMedziCyklami); | |
| } | |
| // There are different ways to query whether a button is pressed. | |
| // // By query each button individually: | |
| // // a(), b(), x(), y(), l1(), etc... | |
| // if (ctl->a()) { | |
| // } | |
| // if (ctl->b()) { | |
| // } | |
| // if (ctl->x()) { | |
| // // Some gamepads like DS3, DS4, DualSense, Switch, Xbox One S, Stadia support rumble. | |
| // // It is possible to set it by calling: | |
| // // Some controllers have two motors: "strong motor", "weak motor". | |
| // // It is possible to control them independently. | |
| // ctl->playDualRumble(0 /* delayedStartMs */, 250 /* durationMs */, 0x80 /* weakMagnitude */, | |
| // 0x40 /* strongMagnitude */); | |
| // } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment