/**
 * Author: Omar Draidrya
 * Date: 2024/05/05
 * This code controls multiple servos using the PCA9685 PWM driver and analog input from potentiometers.
 */

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

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); // Default address 0x40

void setup() {
    Serial.begin(9600);           // Start serial communication for debugging purposes.
    pwm.begin();                  // Initialize the PCA9685 module.
    pwm.setPWMFreq(60);           // Set the PWM frequency suitable for servos.
}

void loop() {
    // Servo 0
    int potValue0 = analogRead(A0);           // Read analog value from potentiometer connected to A0
    int servoPos0 = map(potValue0, 0, 1023, 125, 575); // Map the analog value to servo position
    pwm.setPWM(0, 0, servoPos0);              // Set servo 0 position

    // Servo 1
    int potValue1 = analogRead(A1);           // Read analog value from potentiometer connected to A1
    int servoPos1 = map(potValue1, 0, 1023, 125, 575); // Map the analog value to servo position
    pwm.setPWM(1, 0, servoPos1);              // Set servo 1 position

    // Servo 2
    int potValue2 = analogRead(A2);           // Read analog value from potentiometer connected to A2
    int servoPos2 = map(potValue2, 0, 1023, 125, 575); // Map the analog value to servo position
    pwm.setPWM(2, 0, servoPos2);              // Set servo 2 position

    // Servo 3
    int potValue3 = analogRead(A3);           // Read analog value from potentiometer connected to A3
    int servoPos3 = map(potValue3, 0, 1023, 125, 575); // Map the analog value to servo position
    pwm.setPWM(3, 0, servoPos3);              // Set servo 3 position

    // Servo 4
    int potValue4 = analogRead(A4);           // Read analog value from potentiometer connected to A4
    int servoPos4 = map(potValue4, 0, 1023, 125, 575); // Map the analog value to servo position
    pwm.setPWM(4, 0, servoPos4);              // Set servo 4 position

    // Servo 5
    int potValue5 = analogRead(A5);           // Read analog value from potentiometer connected to A5
    int servoPos5 = map(potValue5, 0, 1023, 125, 575); // Map the analog value to servo position
    pwm.setPWM(5, 0, servoPos5);              // Set servo 5 position

    delay(20);   // Short delay for stability
}