#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_Left_Step = 5;
//Define an array of Left movements of the robot
int Servo_Left [][ALLMATRIX] PROGMEM = {
  //GPIO5, GPIO16, GPIO18, GPIO17, time
  {60, 60, 125, 90, 300}, //The left thigh rotates laterally, the right thigh rotates laterally, and the left calf rotates laterally
  {60, 90, 125, 90, 300}, //Turn the left calf inwards
  {60, 120, 55, 120, 300}, //The left calf rotates inwards, the right thigh rotates inwards, and the right calf rotates laterally
  {60, 90, 55, 90, 300}, //The left calf rotates laterally and the right calf rotates inwards
  {80, 90, 90, 90, 150}, //The left thigh rotates inwards and the right thigh laterally
};

void loop() {
  Servo_PROGRAM_Run(Servo_Left, Servo_Left_Step);
}