servo_calibration_basic.ino

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

int DELAY = 10;
int INCREMENT = 1;
int FREQ = 150;
int LARGE_DELAY = 400;

Adafruit_PWMServoDriver pwmTop = Adafruit_PWMServoDriver(0x40);
Adafruit_PWMServoDriver pwmBottom = Adafruit_PWMServoDriver(0x41);

void setup() {
  pwmTop.begin();
  pwmBottom.begin();
  pwmTop.setOscillatorFrequency(27000000);
  pwmBottom.setOscillatorFrequency(27000000);
  pwmTop.setPWMFreq(FREQ);
  pwmBottom.setPWMFreq(FREQ);

  // if you want to really speed stuff up, you can go into 'fast 400khz I2C' mode
  // some i2c devices dont like this so much so if you're sharing the bus, watch
  // out for this!
  Wire.setClock(400000);

}

void sweepUp(Adafruit_PWMServoDriver pwm, int servo_id, int servo_min, int servo_max) {
  pwm.setPWM(servo_id, 0, servo_min);
  delay(LARGE_DELAY);

  for (int i = servo_min; i < servo_max; i+=5) {
    tick(pwm, servo_id, i);
  }
  pwm.setPWM(servo_id, 0, 0);
}

void sweepDown(Adafruit_PWMServoDriver pwm, int servo_id, int servo_min, int servo_max) {
  pwm.setPWM(servo_id, 0, servo_max);
  delay(LARGE_DELAY);

  for (int i = servo_max; i > servo_min; i-=5) {
    tick(pwm, servo_id, i);
  }
  pwm.setPWM(servo_id, 0, 0);
}

void tick(Adafruit_PWMServoDriver pwm, int servo_id, int value) {
  Serial.println(value);
  pwm.setPWM(servo_id, 0, value);
  delay(DELAY);
}

void loop() {
  for (int servoId = 0; servoId < 3; servoId++) {
    sweepUp(pwmBottom, servoId, 200, 1400);
    sweepDown(pwmBottom, servoId, 200, 1400);
  }
  for (int servoId = 3; servoId < 6; servoId++) {
    sweepDown(pwmBottom, servoId, 200, 1400);
    sweepUp(pwmBottom, servoId, 200, 1400);
  }
  sweepUp(pwmTop, 0, 500, 900);
  sweepDown(pwmTop, 0, 500, 900);
}