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

//TMI8549电机驱动芯片 TMI8549 motor drive chip
#define ENA 5   //使能端，可使用PWM调速 Enable end, can use PWM speed regulation
#define INT1 7  //IN1输入端 IN1 Input end
#define INT2 8  //IN2输入端 IN2 Input end

#define sensor_L 10 //左侧红外传感器 Left infrared sensor
#define sensor_R 9  //右侧红外传感器 Right infrared sensor

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

int SensorLeft;
int SensorRight;
int distance;

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() {
  follow();
}

void follow(){
    SensorLeft = digitalRead(sensor_L); 
    SensorRight = digitalRead(sensor_R);
    distance = GetDistance(); //获取距离
    if(SensorLeft == LOW && SensorRight == HIGH){
      left();      
    }else if(SensorLeft == HIGH && SensorRight == LOW){
      right();
    }else if(SensorLeft == LOW && SensorRight == LOW){
      back();
    }else if(distance >=10 && distance <= 30){
      forward();      
    }else if(distance >30 || (distance >= 5 && distance <10)){
      stop();
    }else if(distance <5){
      back();
    }
}

//前进
void forward(){
  analogWrite(ENA, 120);  //对ENA输入0~255的PWM波进行调速
  digitalWrite(INT1, LOW); 
  digitalWrite(INT2, LOW);
  Serial.println("forward");
}

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

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

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

//右转
void right() {
  analogWrite(ENA, 120);  //对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;
}
