// 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;
int FRK = 4;
int FLK = 5;
int BRK = 6;
int BLK = 7;

float FR_init = 1350;
float FL_init = 1350;
float BR_init = 1440;
float BL_init = 1440;
float FRK_init = 1350;
float FLK_init = 1350;
float BRK_init = 1350;
float BLK_init = 1350;

float SERVOFREQ = 50;
float pulseconstant;

void setup() {
    pulseconstant = (1000000/SERVOFREQ)/4096;

    pwm.begin();
    pwm.setPWMFreq(SERVOFREQ);

    servoWrite(FRleg, FR_init);
    servoWrite(FLleg, FL_init);
    servoWrite(BRleg, BR_init);
    servoWrite(BLleg, BL_init);
    servoWrite(FRK, FRK_init);
    servoWrite(FLK, FLK_init);
    servoWrite(BRK, BRK_init);
    servoWrite(BLK, BLK_init);
}

void loop() {
  forward1();
  back();
  forward2();

}

void servoWrite(uint8_t n, float pulse) {
    float pulsetick = pulse/pulseconstant;
    pwm.setPWM(n, 0, pulsetick);
}

void forward1() {
 servoWrite(FRleg, 1600);
 servoWrite(BLleg, 1600);
 delay(500);
}

void back() {
  servoWrite(FRleg, 1200);
  servoWrite(BLleg, 1200);
  delay(500);
}

void forward2() {
  servoWrite(FLleg, 1600);
  servoWrite(BRleg, 1600);
  delay(500);
}