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

#define FIRMWARE_VERSION "20240815 V1.0"

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_Dance1_Step = 22;
int Servo_Dance1 [][ALLMATRIX] PROGMEM = {
//GPIO5, GPIO16, GPIO18, GPIO17 , time
{50,90,80,90, 300}, //Left thigh to the lateral rotation
{50,130,80,100, 300}, //Left calf to the inward rotation
{80,130,80,100, 300}, //Left thigh to the inward rotation
{60,130,80,100, 300}, //Left thigh to the lateral rotation
{80,130,80,100, 300}, //Left thigh to the inward rotation
{60,130,80,100, 300}, //Left thigh to the lateral rotation
{80,130,80,100, 300}, //Left thigh to the inward rotation
{60,130,80,100, 300}, //Left thigh to the lateral rotation
{80,130,80,100, 300}, //Left thigh to the inward rotation
{60,110,80,105, 300}, //Left thigh to the lateral rotation，Left calf to the inward rotation
{90,90,90,90, 300}, //Action initialization of biped robot
{100,90,125,90, 300}, //Right thigh to the lateral rotation
{100,75,125,50, 300}, //Right calf to the inward rotation
{100,75,100,50, 300}, //Right thigh to the inward rotation
{100,75,120,50, 300}, //Right thigh to the lateral rotation
{100,75,100,50, 300}, //Right thigh to the inward rotation
{100,75,120,50, 300}, //Right thigh to the lateral rotation
{100,75,100,50, 300}, //Right thigh to the inward rotation
{100,75,120,50, 300}, //Right thigh to the lateral rotation
{100,75,100,50, 300}, //Right thigh to the inward rotation
{100,90,120,70, 300}, //Right thigh to the lateral rotation，Right calf to the lateral rotation
{90,90,90,90, 300}, //Action initialization of biped robot
};

void loop() {
  Servo_PROGRAM_Run(Servo_Dance1, Servo_Dance1_Step);
}

