Dynamic Control rev_01

/********* Pleasedontcode.com **********

    Pleasedontcode thanks you for automatic code generation! Enjoy your code!

    - Terms and Conditions:
    You have a non-exclusive, revocable, worldwide, royalty-free license
    for personal and commercial use. Attribution is optional; modifications
    are allowed, but you're responsible for code maintenance. We're not
    liable for any loss or damage. For full terms,
    please visit pleasedontcode.com/termsandconditions.

    - Project: Dynamic Control
    - Source Code NOT compiled for: XIAO ESP32S3
    - Source Code created on: 2025-11-09 21:57:07

********* Pleasedontcode.com **********/

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
    /* ovládá šest servomotorů na G1 G2 G5 G6 G7 G8 */
    /* pomocí Bluetooth aplikace s tím že při startu se */
    /* všechny serva srovnají na 90 stupňů */
/****** END SYSTEM REQUIREMENTS *****/


/* START CODE */

#include <BluetoothSerial.h>
#include <Servo.h>

// Define pins for servos G1, G2, G5, G6, G7, G8
const int servoPins[] = { G1, G2, G5, G6, G7, G8 };
// Initialize servo objects
Servo servos[6];

// Bluetooth serial object
BluetoothSerial SerialBT;

// Create an array for target angles
int targetAngles[6] = {90, 90, 90, 90, 90, 90};

// Buffer for incoming Bluetooth data
String buffer = "";

// Function to attach servos to pins and initialize positions
void initializeServos() {
    for (int i = 0; i < 6; i++) {
        servos[i].attach(servoPins[i]);
        servos[i].write(90); // set to 90 degrees
    }
}

// Function to process received Bluetooth commands
void processCommand(String command) {
    // Expect commands in the format: Gx:angle, e.g., G1:45
    int separatorIndex = command.indexOf(":");
    if (separatorIndex == -1) return; // Invalid command

    String servoId = command.substring(0, separatorIndex);
    String angleStr = command.substring(separatorIndex + 1);
    int angle = angleStr.toInt();

    // Map servo ID to index
    int index = -1;
    if (servoId == "G1") index = 0;
    else if (servoId == "G2") index = 1;
    else if (servoId == "G5") index = 2;
    else if (servoId == "G6") index = 3;
    else if (servoId == "G7") index = 4;
    else if (servoId == "G8") index = 5;

    if (index != -1 && angle >= 0 && angle <= 180) {
        servos[index].write(angle);
        targetAngles[index] = angle;
    }
}

void setup() {
    initializeServos();
    SerialBT.begin("ESP32ServoController"); // Bluetooth device name
    // Initialize buffer
    buffer = "";
}

void loop() {
    if (SerialBT.available()) {
        char incomingChar = SerialBT.read();
        if (incomingChar == '\n') {
            processCommand(buffer);
            buffer = "";
        } else {
            buffer += incomingChar;
        }
    }
}

/* END CODE */