/********* 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: Bluetooth Servos
	- Source Code NOT compiled for: XIAO ESP32S3
	- Source Code created on: 2025-11-09 22:00:33

********* 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 
#include 

// 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 */