/* PCA9685GA1502v1
   08/01/2024

   Test of a GS-1502 Linear Servo with PCA9685
   some quick and dirty code just to see how the board performs.

   Visit http://www.digitaltown.co.uk/projectPCA9685PointControl.php for some better control code
   that does NOT use delay() 


*/

#include "Wire.h"

#include "Adafruit_PWMServoDriver.h"
//PCA 9685 settings


Adafruit_PWMServoDriver servoBoard = Adafruit_PWMServoDriver(0x40);//Address found from I2C scanner
#define USMIN  832 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
//#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
//end pca9685 settings

//sets the position of the servo in degrees.
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
    servoBoard.writeMicroseconds(servo, sendPos);
  }
}

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

  servoBoard.begin();
  servoBoard.setOscillatorFrequency(27000000);
  servoBoard.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

 
  delay(10);//let the above values take effect

  
}



void loop() {
  int q;
 setServoPos(15, 179);
 Serial.println("179");
 delay(2000);
 setServoPos(15, 0);
 Serial.println("0");
 delay(2000);

 for(q=0;q<180;q++){
  setServoPos(15, q);
  delay(50);
 }
 delay(1000);
 for(q=179;q>-1;q--){
  setServoPos(15, q);
  delay(50);
 }
 delay(1000);

}