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

void loop() {
  Servo_PROGRAM_Run(Servo_Right, Servo_Right_Step);
}