#include <Arduino.h>
#include <ACB_Biped_Robot.h>

void setup() {// initialize servo
  servo_5.attach(5, SERVOMIN, SERVOMAX);
  servo_16.attach(16, SERVOMIN, SERVOMAX);
  servo_17.attach(17, SERVOMIN, SERVOMAX);
  servo_18.attach(18, SERVOMIN, SERVOMAX);
  Servo_PROGRAM_Zero();// 90 90 90 90
}

int Servo_Backward_Step = 4;

//Define an array of Backward movements of the robot
int Servo_Backward [][ALLMATRIX] PROGMEM = {
  //GPIO5, GPIO16, GPIO18, GPIO17, time
  {120, 75, 110, 110, 300}, //Right leg up, right leg backward
  {90, 100, 110, 75, 300}, //Right leg landing
  {60, 75, 65, 90, 300}, //Left leg up, left leg backward
  {60, 90, 60, 90, 300}, //Left leg landing
};

void loop() {
  Servo_PROGRAM_Run(Servo_Backward, Servo_Backward_Step);
}