#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <BlynkSimpleESP32.h>

// Create an object for the PCA9685 servo driver
Adafruit_PWMServoDriver pca = Adafruit_PWMServoDriver();

// Define the number of servos and their channels
#define NUM_SERVOS 3

// Define the servo channels
int servo_channels[NUM_SERVOS] = {0, 1, 2};

// Define a function to set the servo angle
void setServoAngle(int channel, int angle) {
  int servoMin = 150; // Min pulse length in microseconds
  int servoMax = 600; // Max pulse length in microseconds
  int pulseLength = map(angle, 0, 180, servoMin, servoMax);
  pca.setPWM(channel, 0, pulseLength);
}

void setup() {
  // Initialize the serial communication at 9600 baud
  Serial.begin(9600);

  // Initialize the PCA9685 servo driver
  pca.begin();
  pca.setPWMFreq(60); // Set the PWM frequency to 60 Hz


   for (int i = 0; i < NUM_SERVOS; i++) {
    setServoAngle(servo_channels[i], 0);
  }

  // Wait for the user to send a command
  while (!Serial.available()) {}
}

void loop() {
  // Read the user's command from the serial monitor
  if (Serial.available() >= 5) {
    String command = Serial.readStringUntil(' ');
    int channel = command.toInt();
    int angle = Serial.readStringUntil('\n').toInt();

    // Check if the channel is valid
    if (channel >= 0 && channel < NUM_SERVOS) {
      
      setServoAngle(servo_channels[channel], angle);
    }
  }
}