Final Draft Documentation: Servos Part

Arduino Code:

#include <Adafruit_PWMServoDriver.h>

#include <Wire.h>

Adafruit_PWMServoDriver pwm0 = Adafruit_PWMServoDriver(0x40);

Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0x42);

Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0x44);

#define servomin 125

#define servomax 575

#define gridSize 5

uint8_t servonum = 0;

void setup() {

  Serial.begin(9600);

  pwm0.begin();

  pwm1.begin();

  pwm2.begin();

  pwm0.setPWMFreq(60);

  pwm1.setPWMFreq(60);

  pwm2.setPWMFreq(60);

}

void loop() {

  int mx = analogRead(A0);

  int my = analogRead(A1);

  for (int y = 0; y < gridSize; y++) {

    for (int x = 0; x < gridSize; x++) {

      int index = (y * gridSize) + x;

      int targetX = map(x, 0, gridSize-1, 0, 1023);

      int targetY = map(y, 0, gridSize-1, 0, 1023);

      int valX = map(abs(mx-targetX), 500, 0, 0, 255);

      int valY = map(abs(my-targetY), 500, 0, 0, 255);

      valX = constrain(valX, 0, 255);

      valY = constrain(valY, 0, 255);

      int opt = map(valX*valY, 0, 255*255, 0, 179);

      convertPWM(index, opt);

    }

  }

}

void convertPWM(int servoindex, float angle) {

  if (servoindex >= 0 && servoindex < 16) {

    pwm0.setPWM(servoindex, 0, map(int(angle), 0, 180, servomin, servomax));

  } else if (servoindex >= 16 && servoindex < 32) {

    pwm1.setPWM(servoindex % 16, 0, map(int(angle), 0, 180, servomin, servomax));

  } else if (servoindex >= 32 && servoindex < 48) {

    pwm2.setPWM(servoindex % 16, 0, map(int(angle), 0, 180, servomin, servomax));

  }

}

Remarks: The above code works with at most 48 servo motors connected to the 3 x chained pca9685 PWM servo drivers, excluding additional servo motors connected to the breakout board.