#define trig 9
#define ech 10
#define motor1 5
#define motor2 6

int duration = 0; 
int distance = 0;
// int current_speed = 0;
// int max_speed = 0;
int ultra_sonic(int trigger, int echo){
  while(1){
    digitalWrite(trigger, LOW);
    delayMicroseconds(2);
    digitalWrite(trigger, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigger, LOW);
    duration = pulseIn(echo, HIGH, 50000);
    distance = duration * 0.034 / 2;
    if (distance!=0){
      return distance;
    }
  }
} 
void setup() {
  pinMode(trig,OUTPUT);
  pinMode(ech,INPUT);
  pinMode(motor1, OUTPUT);
  pinMode(motor2, OUTPUT);
  Serial.begin(9600);
 
}
 
void loop() {
  int distance = ultra_sonic(trig,ech);
  Serial.println(distance);
  if (distance > 40){
    analogWrite(motor1, 255);
    digitalWrite(motor2, LOW);
    distance = ultra_sonic(trig, ech);
  }
  else if (distance > 35 && distance < 40){
    analogWrite(motor1, 125);
    analogWrite(motor2, 0);
    distance = ultra_sonic(trig, ech);
  } 
  else if (distance > 30 && distance < 35){
    analogWrite(motor1, 63);
    analogWrite(motor2, 0);
    distance = ultra_sonic(trig, ech);
  } 
  else if (distance < 30 && distance > 20){
    digitalWrite(motor1, 0);
    analogWrite(motor2, 0);
    distance = ultra_sonic(trig, ech);
  }
  else if (distance < 20){
    digitalWrite(motor1, 0);
    digitalWrite(motor2, 1);
  }
}