#include <Wire.h>                // Required for I2C communication
#include <Adafruit_PWMServoDriver.h> // Library for the PCA9685 PWM driver

// Create an instance of the Adafruit_PWMServoDriver class
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// Define pulse width constants for the servos
#define SERVOMIN  150 // Minimum pulse length (in pulse-width units) for servo to be at 0 degrees
#define SERVOMAX  600 // Maximum pulse length (in pulse-width units) for servo to be at 180 degrees

void setup() {
  Serial.begin(9600);           // Initialize Serial communication at 9600 baud rate for debugging
  pwm.begin();                  // Initialize the PWM driver (PCA9685)
  pwm.setPWMFreq(60);           // Set the PWM frequency to 60 Hz (typical for servos)

  delay(10);                    // Small delay to ensure the PWM driver is ready
}

// Function to set the angle of a specific servo
void setServoAngle(int servoNum, int angle) {
  if (servoNum < 0 || servoNum > 4) {
    Serial.println("Servo number out of range."); // Check if the servo number is valid (0-4)
    return;
  }

  if (angle < 0 || angle > 180) {
    Serial.println("Angle out of range. Use a value between 0 and 180."); // Check if the angle is valid (0-180 degrees)
    return;
  }

  // Calculate the pulse length corresponding to the desired angle
  int pulseLength = map(angle, 0, 180, SERVOMIN, SERVOMAX);
  
  // Set the PWM signal for the specified servo
  pwm.setPWM(servoNum, 0, pulseLength);
}

void loop() {
  // Example of controlling each servo individually

  // Move Servo 0 to 0 degrees
  setServoAngle(0, 0);
  delay(1000); // Wait for 1 second

  // Move Servo 1 to 45 degrees
  setServoAngle(1, 45);
  delay(1000); // Wait for 1 second

  // Move Servo 2 to 90 degrees
  setServoAngle(2, 90);
  delay(1000); // Wait for 1 second

  // Move Servo 3 to 135 degrees
  setServoAngle(3, 135);
  delay(1000); // Wait for 1 second

  // Move Servo 4 to 180 degrees
  setServoAngle(4, 180);
  delay(1000); // Wait for 1 second

}