#include <Servo.h>
#include<Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
#define SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 1300 // this is the 'maximum' pulse length count (out of 4096)
void setup() {
Serial.begin(9600);
Serial.println("16 channel Servo test!");
pwm.begin();
pwm.setPWMFreq(60);
}
void setServoPulse(uint8_t n, double pulse) {
double pulselength;
pulselength = 1000000;
pulselength /= 60;
Serial.print(pulselength); Serial.println(" us per period");
pulselength /= 4096;
Serial.print(pulselength); Serial.println(" us per bit");
pulse *= 1000;
pulse /= pulselength;
Serial.println(pulse);
pwm.setPWM(n, 0, pulse);
}
void loop()
{
delay(4000);
//Serial.println(servonum);
for (uint16_t pulselen = SERVOMIN; pulselen <SERVOMAX/2; pulselen++) {
pwm.setPWM(0, 0, pulselen);
Serial.println(0);
delay(4);
}
for (uint16_t pulselen = SERVOMIN; pulselen <SERVOMAX/5 ; pulselen++) {
pwm.setPWM(2, 0, pulselen);
pwm.setPWM(4, 0, pulselen);
delay(4);
Serial.println(1);
}
for (uint16_t pulselen = SERVOMAX/5; pulselen >SERVOMIN; pulselen--)
{
pwm.setPWM(2, 0, pulselen);
pwm.setPWM(4, 0, pulselen);
delay(4);
Serial.println(2);
}
for (uint16_t pulselen =SERVOMIN; pulselen <SERVOMAX/5 ; pulselen++)