SumoRobot-Fast-Motors-Code

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

#define echoPin 11//7 // Echo Pin
#define trigPin 12//8 // 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() {
  //Serial.begin (9600);
  delay(5000);
  for (int i = 5; i < 11; i ++)
  {
    pinMode(i, OUTPUT);
  }
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(sensorPin, INPUT);
  pinMode(LEDPin, OUTPUT); // Use LED indicator (if required)

  front();
  delay(300);
}

void loop() {

  sensorState = digitalRead(sensorPin);
  // if the sensorState is HIGH:
  if (sensorState == HIGH) {
    digitalWrite(LEDPin, HIGH);
    fight();
  }
  else {
    digitalWrite(LEDPin, LOW);
    back();
    delay(200);
    left();
    delay(500);
    //stopM();
    //delay(250);
  }
  /*
    if (black() == 0) {
    //digitalWrite(LEDPin, HIGH);
    right();
    delay(250);
    front();
    delay(250);
    fight();
    } else if (black() == 1) {
    digitalWrite(LEDPin, LOW);
    back();
    delay(250);
    left();
    delay(250);
    stopM();
    delay(250);
    }*/

}





















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

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

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

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

void left() { //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) {
    /* Send a negative number to computer and Turn LED ON
      to indicate "out of range" */
    //Serial.println(distance);
    digitalWrite(LEDPin, HIGH);
    front();
    delay(200);
  }
  else {
    /* Send the distance to the computer using Serial protocol, and
      turn LED OFF to indicate successful reading. */
    //Serial.println(distance);
    digitalWrite(LEDPin, LOW);
    left();
    delay(500);
  }
  //Delay 50ms before next reading.
  //delay(5);
}