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

#define FIRMWARE_VERSION "20240815 V1.0"

void setup() {    //initialize servo
  Serial.begin(115200);  //set the baud rate to 115200
  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
}
//Forward
 int Servo_Forward_Step = 4;
 int Servo_Forward [][ALLMATRIX] PROGMEM = {
  {70, 110, 70, 120, 300},//GPIO5, GPIO16, GPIO18, GPIO17, time
  {60, 90, 60, 90, 300},
  {110, 50, 110, 60, 300},
  {105, 90, 105, 90, 300},
  
};
//Backward
 int Servo_Backward_Step = 4;
 int Servo_Backward [][ALLMATRIX] PROGMEM = {
  {120, 75, 110, 110, 300},//GPIO5, GPIO16, GPIO18, GPIO17, time
  {90, 100, 110, 75, 300},
  {60, 75, 65, 90, 300},   
  {60, 90, 60, 90, 300}, 
};
//Left
 int Servo_Left_Step = 5;
 int Servo_Left [][ALLMATRIX] PROGMEM = {
  {60, 60, 125, 90, 300},//GPIO5, GPIO16, GPIO18, GPIO17, time
  {60, 90, 125, 90, 300},
  {60, 120, 55, 120, 300},
  {60, 90, 55, 90, 300},
  {80, 90, 90, 90, 150},
};
//Right
 int Servo_Right_Step = 5;
 int Servo_Right [][ALLMATRIX] PROGMEM = {
  {60, 90, 120, 120, 300}, //GPIO5, GPIO16, GPIO18, GPIO17, time
  {60, 90, 120, 90, 300},
  {120, 60, 120, 60, 300},
  {120, 90, 120, 90, 300},
  {90, 90, 80, 90, 150},
};
// Stop
 int Servo_Stop_Step = 1;
 int Servo_Stop [][ALLMATRIX] PROGMEM = {
  {90, 90, 90, 90, 300}, //GPIO5, GPIO16, GPIO18, GPIO17, time
};

void loop() {
    if (Serial.available()) {//Determine whether there is serial port data
      String input = Serial.readStringUntil('\n');
      String action = input; 
      action.trim();
      Serial.println(action);//Print the input serial port data
      if (action == "forward") {//When "forward" is input, the robot moves forward
        Servo_PROGRAM_Run(Servo_Forward, Servo_Forward_Step);
      } 
      else if (action == "backward") {//When "backward" is input, the robot moves backward
        Servo_PROGRAM_Run(Servo_Backward, Servo_Backward_Step);
      } 
      else if (action == "left") {//When "left" is input, the robot rotates to the left
        Servo_PROGRAM_Run(Servo_Left, Servo_Left_Step);
      } 
      else if (action == "right") {//When "right" is input, the robot rotates to the right
        Servo_PROGRAM_Run(Servo_Right, Servo_Right_Step);
      } 
      Servo_PROGRAM_Run(Servo_Stop, Servo_Stop_Step);
  }
}
