sumo-test-ultrasonic

#define sensorPin 4           // the number of the sensor pin
int sensorState = 0;         // variable for reading the sensor status

#define echoPin 11 // Echo Pin
#define trigPin 12 // Trigger Pin
int maximumRange = 20; // Maximum range needed
long duration, distance; // Duration used to calculate distance


#define LEDPin 13 // Onboard LED

int IN1 = 5;
int IN2 = 6;
int IN3 = 7;
int IN4 = 8;
int ENA = 9;
int ENB = 10;

void setup() {
  delay(5000);
  for (int i = 5; i < 11; i ++)
  {
    pinMode(i, OUTPUT);
  }
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(sensorPin, INPUT);
  pinMode(LEDPin, OUTPUT);

  front();
  delay(300);
}

void loop() {

  fight();
  delay(50);
/*
  sensorState = digitalRead(sensorPin);
  // if the sensorState is HIGH:
  if (sensorState == HIGH) {
    digitalWrite(LEDPin, HIGH);
    //fight();
    ///////////////////
    stopM();
    delay(1000);
    ///////////////////
  }
  else {
    digitalWrite(LEDPin, LOW);
    back();
    delay(200);
    ///////////////////
    //left();
    //delay(500);
    ///////////////////
  }*/
}





















void stopM() {
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}

void left() {
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 1);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 1);
  analogWrite(ENA, 255);
  analogWrite(ENB, 255);
}

void right() {
  digitalWrite(IN1, 1);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 1);
  digitalWrite(IN4, 0);
  analogWrite(ENA, 255);
  analogWrite(ENB, 255);
}

void back() {
  digitalWrite(IN1, 1);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 1);
  analogWrite(ENA, 150);
  analogWrite(ENB, 150);
}

void front() {
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 1);
  digitalWrite(IN3, 1);
  digitalWrite(IN4, 0);
  analogWrite(ENA, 150);
  analogWrite(ENB, 150);
}






void fight() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);

  //Calculate the distance (in cm) based on the speed of sound.
  distance = duration / 58.2;

  if (distance <= maximumRange) {
    Serial.println("Attacking !!");
    digitalWrite(LEDPin, HIGH);
    front();
    delay(200);
  }
  else {
    Serial.println(distance);
    digitalWrite(LEDPin, LOW);
    left();
    delay(500);
  }
  //Delay 50ms before next reading.
  //delay(5);
}