/* PCA9685ServoV4
 *  26/12/23
 *  
 This sketch is controlling 2 servos, one on pin 0 the 2nd on pin 3
 *  
 * PCA9685 Board with MG90 servo
 * 
 * 
 * 
SDA/SCL connections 

Arduino UNO: A4 (SDA), A5 (SCL) 
Arduino Mega 2560: 20 (SDA), 21 (SCL)  
ESP32: 21(SDA), 22 (SCL)

 */


#include "Wire.h"
#include "Adafruit_PWMServoDriver.h"

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
// you can also call it with a different address and I2C interface
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, Wire);

#define USMIN  600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX  2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates

// our servos
const int servo0 = 0;
const int servo3 = 3;

void setServoPos(int servo, int pos){
  //This first bit of code makes sure we are not trying to set the servo outside of limits
  int sendPos;
  if(pos > 179){
    pos = 179;
  }
  if(pos < 0){
    pos = 0;
  }
  sendPos = USMIN + ((USMAX - USMIN)/180 * pos);
  if(servo > -1 && servo < 16){//only try to move valid servo addresses
    pwm.writeMicroseconds(servo, sendPos);
  }
}

void setup() {
  Serial.begin(9600);
  Serial.println("PCA9685ServoV4");

  pwm.begin();
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates
  delay(10);
}

void loop() {
    setServoPos(servo0,0);//0 Degrees
  delay(500);
  setServoPos(servo0,90);//90 degrees
  delay(500);
  setServoPos(servo0,179);//179 degrees
  delay(500);
  setServoPos(servo0,90);//90 degrees
  delay(500);

  setServoPos(servo3,0);//0 Degrees
  delay(500);
  setServoPos(servo3,90);//90 degrees
  delay(500);
  setServoPos(servo3,179);//179 degrees
  delay(500);
  setServoPos(servo3,90);//90 degrees
  delay(500);
}