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

#define FIRMWARE_VERSION "20240815 V1.0"

void setup() { 
  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
  Ultrasonic_Init();
}

// 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}, 
};

// Stop
int Servo_Stop_Step = 1;
int Servo_Stop [][ALLMATRIX] PROGMEM = {
  {90, 90, 90, 90, 300},//GPIO5, GPIO16, GPIO18, GPIO17 , time
};

void loop() {
  UT_distance = Ranging(Trig_PIN, Echo_PIN);
  //When the ultrasonic detection distance is less than 15CM, the robot moves backward
  if (UT_distance < 15) {
    Servo_PROGRAM_Run(Servo_Backward, Servo_Backward_Step);// Backward
  }
  //When the ultrasonic detection distance is 15~20CM, the robot stops moving
  else if (15 <= UT_distance && UT_distance <= 20) {
    Servo_PROGRAM_Run(Servo_Stop, Servo_Stop_Step);// Stop
  }
  //When the ultrasonic detection distance is 20~35CM, the robot moves forward
  else if (20 <= UT_distance && UT_distance < 35) {
    Servo_PROGRAM_Run(Servo_Forward, Servo_Forward_Step);// Forward
  }
  else{
    Servo_PROGRAM_Run(Servo_Stop, Servo_Stop_Step); // Stop
  } 
}
