Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
#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);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment