/*************************************
 *     TIME:2023.3.9
 *     Development Team: Zhiyi Technology Co., Ltd.
 * 
 *  **************************************/
#include <Servo.h>
Servo myservo;//设置一个舵机

//TMI8549电机驱动芯片
#define ENA 5   //使能端，可使用PWM调速
#define INT1 7  //IN1输入端
#define INT2 8  //IN2输入端

#define sensor_L 10 //左侧红外传感器
#define sensor_R 9  //右侧红外传感器

//超声波传感器引脚设置
int Echo = 12;//Pin to D12
int Trig = 13;//Pin to D13

int SensorLeft;
int SensorRight;
int rightDistance = 0, leftDistance = 0, middleDistance = 0;  //定义超声波三个方向距离

void setup() {
  Serial.begin(9600);
  myservo.attach(11,700,2400);//设置舵机引脚为D11
  myservo.write(80); //初始化舵机角度
  pinMode(ENA, OUTPUT);   //引脚设置输出模式
  pinMode(INT1, OUTPUT);  //引脚设置输出模式
  pinMode(INT2, OUTPUT);  //引脚设置输出模式
  pinMode(Echo, INPUT);
  pinMode(Trig, OUTPUT); 
  pinMode(sensor_L, INPUT);
  pinMode(sensor_R, INPUT);
    
  delay(100); 
}

//INT1,INT2输出控制逻辑
  //INT1   INT2   OUT1    OUT2    OUT3    OUT4
  //0      0      1       0       1       0     前进
  //0      1      1       0       0       1     右转
  //1      0      0       1       1       0     左转
  //1      1      0       1       0       1     后退

void loop() {
  Ultrasonic();
}

//前进
void forward(){
  analogWrite(ENA, 120);  //对ENA输入0~255的PWM波进行调速
  digitalWrite(INT1, LOW); 
  digitalWrite(INT2, LOW);  
}
//前进慢速
void forward_U(){
  analogWrite(ENA, 70);  //The PWM wave from ENA input 0 to 255 is speed-regulated
  digitalWrite(INT1, LOW); 
  digitalWrite(INT2, LOW);
}

//后退
void back() {
  analogWrite(ENA, 120);  //对ENA输入0~255的PWM波进行调速
  digitalWrite(INT1,HIGH);  //后退
  digitalWrite(INT2,HIGH);  
}

void stop(){
  analogWrite(ENA, 0);  //对ENA输入0~255的PWM波进行调速,设置为0停车
}

//左转
void left() {
  analogWrite(ENA, 150);  //对ENA输入0~255的PWM波进行调速  
  digitalWrite(INT1,HIGH);  //左转
  digitalWrite(INT2,LOW);

}

//右转
void right() {
  analogWrite(ENA, 150);  //对ENA输入0~255的PWM波进行调速
  digitalWrite(INT1,LOW);   //右转
  digitalWrite(INT2,HIGH);

}

float GetDistance()
{
    float distance;
    // Send a low short pulse to Trig to trigger the ranging
    digitalWrite(Trig, LOW); //Send a low level to Trig
    delayMicroseconds(2); 
    digitalWrite(Trig, HIGH); 
    delayMicroseconds(10);
    digitalWrite(Trig, LOW);
    distance = pulseIn(Echo, HIGH) / 58.00;
    //Serial.print("Distance = ");
    //Serial.println(distance);//The serial output distance is converted into cm
	  return distance;
}

void Ultrasonic(){
    SensorLeft = digitalRead(sensor_L); 
    SensorRight = digitalRead(sensor_R);       
    middleDistance = GetDistance(); // Get the intermediate distance
    if(middleDistance < 25){
      back();
      delay(50);
      stop();
      delay(200);
      myservo.write(145);// Turn left
      delay(300);  
      leftDistance = GetDistance();
      delay(100);
      myservo.write(35);// Turn right
      delay(300);          
      rightDistance = GetDistance();
      delay(100);
      myservo.write(90);// Back to positive
      delay(500);

      if(leftDistance > rightDistance){   // Turn left
        left();
        delay(380);        
      }else{   // Turn right
        right();
        delay(380);          
      }         
    }else {    
      if((SensorLeft  == 0) && (SensorRight == 0)){
          back();
          delay(100);
      }else if((SensorLeft  == 0) && (SensorRight == 1)){
          right();
          delay(50);                 
      }else if((SensorLeft  == 1) && (SensorRight == 0)){
          left();
          delay(50);        
      }else if((SensorLeft  == 1) && (SensorRight == 1)){
        if(middleDistance > 50){
          forward();
        }else if(middleDistance <= 50){
          forward_U();
        }
      }
    }
}