// include needed librarys

#include <Adafruit_PWMServoDriver.h>
#include <Wire.h>
// create servo driver (defaults to addr 40)
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// give meaningful names to each servo channel
int FRleg = 0;
int FLleg = 1;
int BRleg = 2;
int BLleg = 3;


// simple [channel,angle] list, terminated with a -1
int center[] = {  FRleg, 90,
                FLleg, 90,
                BRleg, 90,
                BLleg, 90,
                -1};

void setup()
{

  Serial.begin(9600);
  Serial.println("robo dog center test");

  pwm.begin();
  pwm.setPWMFreq(50);
}

// helper function that uses angles rather than pulse ticks
void setAngle( int channel, int angle )
{
  // 4096 ticks is 20,000 us
  // Angle 0 is 1,000 us
  // angle 180 is 2,000 us
  long ticks = ((1000L + (1000L*angle/180L))*4096L)/20000L;
  // update the servo channel with the new pusle
  pwm.setPWM(channel, 0, ticks);
}

// helper function to execute a frame list;
void execute( int *cmd )
{
  while( *cmd >= 0 )
  {
    setAngle( *cmd++, *cmd++);
  }
}

// main loop
void loop()
{
    // set servos to frame 1 positions
    execute( center );

}