#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver board1 = Adafruit_PWMServoDriver(0x40);

#define SERVOMIN  125 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  575 // this is the 'maximum' pulse length count (out of 4096)
int servoNumber = 0;


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial.println("32 channel Servo test!");
  Wire.begin();
  board1.begin();
  board1.setPWMFreq(60);
  board1.setPWM(1, 0, angleToPulse(90) );
  board1.setPWM(2, 0, angleToPulse(120) );
  board1.setPWM(3, 0, angleToPulse(90) );
  board1.setPWM(4, 0, angleToPulse(90) );
  //board1.setPWM(5, 0, angleToPulse(90) );
  //board1.setPWM(6, 0, angleToPulse(90) );

}

void loop() {
  moveUp();
  moveDown();
  moveRight();
  moveLeft();
  //board1.setPWM(1, 0, angleToPulse(10) );//10-170
  //board1.setPWM(2, 0, angleToPulse(120));//120-170
  //board1.setPWM(3, 0, angleToPulse(90) );//90-170
  //board1.setPWM(4, 0, angleToPulse(90) );//90-170
  //board1.setPWM(5, 0, angleToPulse(90) );
  //board1.setPWM(6, 0, angleToPulse(90) );
  //delay(10000);
  //board1.setPWM(1, 0, angleToPulse(170) );
  //board1.setPWM(2, 0, angleToPulse(170) );
  //board1.setPWM(3, 0, angleToPulse(170) );
  //board1.setPWM(4, 0, angleToPulse(170) );
  //board1.setPWM(5, 0, angleToPulse(120) );
  //board1.setPWM(6, 0, angleToPulse(80)  );
  //delay(10000);
}

void moveUp() {
  //board1.setPWM(1, 0, angleToPulse(10) );//10-170
  board1.setPWM(2, 0, angleToPulse(140));//120-170
  //board1.setPWM(3, 0, angleToPulse(90) );//90-170
  //board1.setPWM(4, 0, angleToPulse(90) );//90-170
  //board1.setPWM(5, 0, angleToPulse(90) );
  //board1.setPWM(6, 0, angleToPulse(90) );
  delay(5000);
}

void moveDown() {
  //board1.setPWM(1, 0, angleToPulse(10) );//10-170
  board1.setPWM(2, 0, angleToPulse(100));//120-170
  //board1.setPWM(3, 0, angleToPulse(90) );//90-170
  //board1.setPWM(4, 0, angleToPulse(90) );//90-170
  //board1.setPWM(5, 0, angleToPulse(90) );
  //board1.setPWM(6, 0, angleToPulse(90) );
  delay(5000);
}

void moveRight() {
  board1.setPWM(1, 0, angleToPulse(10) );//10-170
  //board1.setPWM(2, 0, angleToPulse(100));//120-170
  //board1.setPWM(3, 0, angleToPulse(90) );//90-170
  //board1.setPWM(4, 0, angleToPulse(90) );//90-170
  //board1.setPWM(5, 0, angleToPulse(90) );
  //board1.setPWM(6, 0, angleToPulse(90) );
  delay(5000);
}

void moveLeft() {
  board1.setPWM(1, 0, angleToPulse(170) );//10-170
  //board1.setPWM(2, 0, angleToPulse(100));//120-170
  //board1.setPWM(3, 0, angleToPulse(90) );//90-170
  //board1.setPWM(4, 0, angleToPulse(90) );//90-170
  //board1.setPWM(5, 0, angleToPulse(90) );
  //board1.setPWM(6, 0, angleToPulse(90) );
  delay(5000);
}

int angleToPulse(int ang) {
  int pulse = map(ang, 0, 180, SERVOMIN, SERVOMAX); // map angle of 0 to 180 to Servo min and Servo max
  //Serial.print("Angle: "); Serial.print(ang);
  // Serial.print(" pulse: "); Serial.println(pulse);
  return pulse;
}