#include <DistanceSRF04.h> //引用超声库
#include <Servo.h>        //引用舵机库
DistanceSRF04 Dist;       //定义超声
Servo myservo;            //定义舵机

int pinI1 = 7;            //定义电机I1接口
int pinI2 = 8;            //定义电机I2接口
int pinI3 = 9;            //定义电机I3接口
int pinI4 = 10;           //定义电机I4接口
int EA = 5;               //定义电机使能
int EB = 6;               //定义电机使能
int distance1;
int distance2;            //定义超声距离2、右边
int distance3;
void setup() {
  pinMode(pinI1, OUTPUT);
  pinMode(pinI2, OUTPUT);
  pinMode(pinI3, OUTPUT);
  pinMode(pinI4, OUTPUT);
  Dist.begin(4, 3);
  myservo.attach(11);        //舵机端口
  Serial.begin(9600);
}

void loop() {
  myservo.write(90);
  distance1 = Dist.getDistanceCentimeter();
  //Serial.println(distance1);
  //digitalWrite(pinI4, LOW); 
  //digitalWrite(pinI3, HIGH);
  //digitalWrite(pinI1, LOW); 
  //digitalWrite(pinI2, HIGH);
  if (distance1 > 0 && distance1 < 20)
  {
    //刹车
    digitalWrite(pinI4, LOW); 
    digitalWrite(pinI3, LOW);
    digitalWrite(pinI1, LOW); 
    digitalWrite(pinI2, LOW);
    delay(500);
    
    myservo.write(0);
    delay(1000);
    distance2 = Dist.getDistanceCentimeter();
    delay(500);
    
    myservo.write(90);
    delay(1000);   
    
    myservo.write(180);
    delay(1000);
    distance3 = Dist.getDistanceCentimeter();
    delay(500);
    
    myservo.write(90);
    delay(1000);
  
    
           if (distance3 > distance2 || distance3 < 0)
              //左转
              {
                 Serial.println(distance3);
                 Serial.println(distance2);
                 
                 analogWrite(EA, 180);
                 analogWrite(EB, 150);
                 digitalWrite(pinI4, LOW); 
                 digitalWrite(pinI3, HIGH);
                  digitalWrite(pinI1, HIGH); 
                 digitalWrite(pinI2, LOW);
                 delay(500);
                }

                    else 
                        if (distance3 < distance2 || distance2 < 0)
                             {
                                //右转
                                    analogWrite(EA, 180);
                                    analogWrite(EB, 150);
                                    digitalWrite(pinI4, HIGH); 
                                    digitalWrite(pinI3, LOW);
                                    digitalWrite(pinI1, LOW); 
                                    digitalWrite(pinI2, HIGH);
                                    delay(500);
                               }
  }  
                                  
    else
     {
       analogWrite(EA, 180);
       analogWrite(EB, 150);
      digitalWrite(pinI4, LOW); 
      digitalWrite(pinI3, HIGH);
      digitalWrite(pinI1, LOW); 
      digitalWrite(pinI2, HIGH);
    }
 

}