Motors do not respond to input from ultrasound sensors.

I am trying to make a autonomous rover for an upcoming competition for my school, and am having trouble with some of my code. At least I think it's the code that Is the problem.

Simply, there are three ultrasound sensors that get their distances reported ever 1/10 of a second. These values then go through a few "if" statements to see what motors should/should not be enabled. The sensors work fine, but none of my motors seem to be working. Any advice is greatly appreciated!

#define trigPin1 13
#define echoPin1 12
#define trigPin2 11
#define echoPin2 10
#define trigPin3 9
#define echoPin3 8
#define FrontRightMotor 2
#define FrontLeftMotor 3
#define BackRightMotor 4
#define BackLeftMotor 5

long duration, distance, RightSensor,BackSensor,FrontSensor,LeftSensor;

void setup()
{
  Serial.begin (9600);
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
  pinMode(FrontRightMotor, OUTPUT);
  pinMode(FrontLeftMotor, OUTPUT);
  pinMode(BackRightMotor, OUTPUT);
  pinMode(BackLeftMotor, OUTPUT);
}

void loop() 
{
  SonarSensor(trigPin1, echoPin1);
  RightSensor = distance;
  SonarSensor(trigPin2, echoPin2);
  LeftSensor = distance;
  SonarSensor(trigPin3, echoPin3);
  FrontSensor = distance;

  Serial.print(LeftSensor);
  Serial.print(" - ");
  Serial.print(FrontSensor);
  Serial.print(" - ");
  Serial.println(RightSensor);

  if (RightSensor < 5)
  {
    digitalWrite(FrontRightMotor, HIGH);
    digitalWrite(BackRightMotor, HIGH);
    digitalWrite(FrontLeftMotor, LOW);
    digitalWrite(BackLeftMotor, LOW);
  }
  else if (LeftSensor < 5)
  {
    digitalWrite(FrontRightMotor, LOW);
    digitalWrite(BackRightMotor, LOW);
    digitalWrite(FrontLeftMotor, HIGH);
    digitalWrite(BackLeftMotor, HIGH);
  }
  else if (FrontSensor < 5)
  {
    if(LeftSensor < 5)
    {
      digitalWrite(FrontRightMotor, LOW);
      digitalWrite(BackRightMotor, LOW);
      digitalWrite(FrontLeftMotor, HIGH);
      digitalWrite(BackLeftMotor, HIGH);
    }
    else if(RightSensor < 5)
    {
      digitalWrite(FrontRightMotor, HIGH);
      digitalWrite(BackRightMotor, HIGH);
      digitalWrite(FrontLeftMotor, LOW);
      digitalWrite(BackLeftMotor, LOW);
    }
  }
  else 
  {
    digitalWrite(FrontRightMotor, HIGH);
    digitalWrite(BackRightMotor, HIGH);
    digitalWrite(FrontLeftMotor, HIGH);
    digitalWrite(BackLeftMotor, HIGH);
  }

}

void SonarSensor(int trigPin,int echoPin)
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
}

I am using a modified version of the code that is listed here: Interfacing of Multiple Ultrasonic Sensor With Arduino - The Engineering Projects

Add more Serial.println() calls to your code and then run it with the Serial Monitor open so you can see what is happening in the program.

Have you written a simple test program to just run the motors?

Post a circuit diagram showing how your motors are controlled (motor drivers/ESCs/MOSFETs) and particularly how they are powered.

Steve

slipstick:
Have you written a simple test program to just run the motors?

Post a circuit diagram showing how your motors are controlled (motor drivers/ESCs/MOSFETs) and particularly how they are powered.

Steve

Yes I have run a simple test program. All I have is a 5V relay shield from DFRobot plugged into an arduino uno. One wire goes from digital port 2, and the other wire goes straight to ground. No complicated circuts or anything.

The sensors work fine, but none of my motors seem to be working. Any advice is greatly appreciated!

Others have given you good suggestions for serial monitor debugging and motor testing.

I do see a problem in you logic tree. With the if and else if conditions being exclusive(as opposed to just using if) you can never reach the case where FrontSensor <5 and either of the other two sensors are also <5.

Are your relays active HIGH?

Thank you to everyone who offered their help. I have found a solution using transistors. It was brought to my attention that a digital pin can not produce enough Amps to run a motor by itself. Thank you again for the help to those who offered it.