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

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

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

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() {
  UT_distance = Ranging(Trig_PIN, Echo_PIN);
  //When the ultrasonic detection distance is less than 15CM, the robot stops moving and then moves backward 6 times
  if (UT_distance <= 15) {
    Servo_PROGRAM_Run(Servo_Stop, Servo_Stop_Step);
    for(int i=1;i<=6;i++){
        Servo_PROGRAM_Run(Servo_Backward, Servo_Backward_Step);
    }
  } 
  // When the ultrasonic detection distance is 15-20cm, the robot will randomly choose to turn left or right
  if (UT_distance > 15 && UT_distance <= 20){
    int randNumber = random(1, 3);//Produces a random integer of 1 or 2
      switch (randNumber) {
        case 1://When the random number is 1
        for(int i=1;i<=10;i++){//Rotate to the right 10 times
          Servo_PROGRAM_Run(Servo_Right, Servo_Right_Step);
    }
          delay(500);
          break;
        case 2://When the random number is 2
        for(int i=1;i<=10;i++){//Rotate to the left 10 times
          Servo_PROGRAM_Run(Servo_Left, Servo_Left_Step);
        }
          delay(500);
          break;
      }
    }
  
  else {//When the ultrasonic detection distance is greater than 20CM, the robot moves forward
    Servo_PROGRAM_Run(Servo_Forward, Servo_Forward_Step);
  }
}

