Ultrasonic sensor not measuring distance

superf1boi · February 1, 2025, 3:14pm

I think I'm going to switch back to my old setup because it is more simple and more compatible for my four motor robot. Except I'm going to remove my 4xAA battery pack.:

+-----------------------+           +-----------------------+
|    6xAA Battery Pack  |           |  4xAA Battery Pack   |
|   +V            GND   |           |   +V            GND  |
+-----------------------+           +-----------------------+
        |                                |       |
        |                                |       |
        V                                V       |
+--------------------------------------+ |       |
|          Arduino Motor Shield        | |       |
|                                      | |       |
|  VIN <-------------------------------+ |       |
|  GND <--------------------------------+-------+|
|                                      |         |
|  M1 ---> DC Motor 1                  |         |
|  M2 ---> DC Motor 2                  |         |
|  M3 ---> DC Motor 3                  |         |
|  M4 ---> DC Motor 4                  |         |
|                                      |         |
|  Pin 9 ---> Servo Signal             |         |
|                                      |         |
|  Pin 6 ---> Ultrasonic Sensor TRIG   |         |
|  Pin 7 ---> Ultrasonic Sensor ECHO   |         |
|  Pin A2 --> IR Sensor                |         |
+--------------------------------------+         |
         |   |                                    |
         |   |                                    |
         V   V                                    |
+-------------------------+                       |
|       Breadboard        |                       |
|                         |                       |
|    +V Rail --> Sensors  |                       |
|    GND Rail --> Sensors |                       |
+-------------------------+                       |
         |   |                                    |
         V   V                                    |
+-------------------------+                       |
|       Servo Motor       |                       |
|   +V  <-----------------+                       |
|   GND <-----------------+                       |
|   Signal <--------------+ Pin 9 on Motor Shield |
+-------------------------+                       |

jim-p · February 1, 2025, 3:17pm

What?
Your schematic shows that you are using the Arduino motor shield.

If you want help you need to provide a correct and accurate schematic of the actual connections and provide a list of the components that you are actually using.

superf1boi · February 1, 2025, 3:21pm

Sorry for the misunderstanding. The one you are looking at is my old schematic that I just posted because I am considering switching back to it.

This is the new one:

Ultrasonic Sensor (HC-SR04)
    ----------------------------
    |  VCC  | ---- (5V) ---- Arduino 5V  
    |  GND  | ---- (GND) ---- Arduino GND  
    |  TRIG | ---- (A0)  ---- Arduino A0  
    |  ECHO | ---- (A1)  ---- Arduino A1  
    ----------------------------

    L298N Motor Driver #1 (Left Motors)
    ------------------------------------
    | 12V   | ---- (Battery +)  
    | GND   | ---- (Battery - & Arduino GND)  
    | 5V    | ---- (Enable jumper ON)  
    | ENA   | ---- (PWM Pin 9) (Arduino PWM Output)  
    | IN1   | ---- (Pin 7) (Motor direction)  
    | IN2   | ---- (Pin 8) (Motor direction)  
    | OUT1  | ---- (Left Motor +)  
    | OUT2  | ---- (Left Motor -)  
    ------------------------------------

    L298N Motor Driver #2 (Right Motors)
    ------------------------------------
    | 12V   | ---- (Battery +)  
    | GND   | ---- (Battery - & Arduino GND)  
    | 5V    | ---- (Enable jumper ON)  
    | ENB   | ---- (PWM Pin 10) (Arduino PWM Output)  
    | IN3   | ---- (Pin 5) (Motor direction)  
    | IN4   | ---- (Pin 6) (Motor direction)  
    | OUT3  | ---- (Right Motor +)  
    | OUT4  | ---- (Right Motor -)  
    ------------------------------------

    Arduino Uno Connections
    ------------------------------------
    | A0   | ---- Trigger (Ultrasonic)  
    | A1   | ---- Echo (Ultrasonic)  
    |  7   | ---- IN1 (L298N Left)  
    |  8   | ---- IN2 (L298N Left)  
    |  5   | ---- IN3 (L298N Right)  
    |  6   | ---- IN4 (L298N Right)  
    |  9   | ---- ENA (L298N Left - PWM)  
    | 10   | ---- ENB (L298N Right - PWM)  
    | GND  | ---- Common Ground with Motor Driver & Battery  
    ------------------------------------

jim-p · February 1, 2025, 3:23pm

So disconnect the drivers, power the Uno via USB and see if the ultrasonic sensor works with the code you show in post #1.

jremington · February 1, 2025, 3:23pm

Your misuse of the motor shield is the problem.

Good luck with your project!

TomGeorge · February 1, 2025, 3:30pm

Hi, @superf1boi

Can you please post a copy of your circuit, a picture of a hand drawn circuit in jpg, png?
Hand drawn and photographed is perfectly acceptable.
Please include ALL hardware, power supplies, component names and pin labels.

Its so much easier to read and produce.

Is there a reason you are not using the NewPing library for the UltraSonics?

https://docs.arduino.cc/libraries/newping/

Tom....

JCA34F · February 1, 2025, 3:31pm

With the L298's Voltage regulator dropping 12V to 5V, it may only be able to supply about 160mA to feed the Arduino and whatever it's powering.

superf1boi · February 1, 2025, 3:37pm

Thanks for the help everyone! @TomGeorge, I will draw a picture of my circuit, as soon as possible! Also, my ultrasonic sensor works perfectly fine with it being just alone with my Arduino Uno. Should I use a different motor driver becaues @JCA34F said that the l298n can't supply that much gurrent to the arduino uno?

jim-p · February 1, 2025, 3:45pm

No it's fine for now. So now connect one of the drivers but power the Uno with the USB, NOT from the driver.

Post the entire code you use with the ultrasonic sensor and the two L298 drivers.
I don't need a schematic just yet.

superf1boi · February 1, 2025, 4:04pm

Omg I think it's because I taped my ultrasonic sensor to the sensor holder and I think it might have damaged the sensor. It works now with another sensor. I'm making a robot that follows an IR beacon.

#include <IRremote.hpp>
#include <Servo.h>

// Motor Driver Pins (L298N)
int IN1_1 = 6;  // Motor 1 direction control
int IN2_1 = 7;  // Motor 1 direction control
int IN3_1 = 8;  // Motor 2 direction control
int IN4_1 = 9;  // Motor 2 direction control

int IN1_2 = 12; // Motor 3 direction control
int IN2_2 = 13; // Motor 3 direction control
int IN3_2 = 4;  // Motor 4 direction control
int IN4_2 = 2;  // Motor 4 direction control


const int IR_RECEIVER_PIN = A3;  
const int TRIG_PIN = A0;        
const int ECHO_PIN = A1;         
const int SERVO_PIN = A2;         

// Obstacle Detection Threshold
const int OBSTACLE_DISTANCE = 15;  // Stop if an obstacle is within 15 cm

// Signal Thresholds
const int STRONG_SIGNAL_THRESHOLD = 1000;
const int WEAK_SIGNAL_THRESHOLD = 200;

// Servo setup
Servo ultrasonicServo;

// Flag to track turning state
bool isTurning = false;

// Function Prototypes
void testMotors();
bool isObstacleDetected();
int measureDistance();
void avoidObstacle();
void adjustDirection();
void moveForward();
void moveBackward();
void turnLeft();
void turnRight();
void stopMotors();

void setup() {
  Serial.begin(9600);
  Serial.println("Initializing...");

  // Motor Driver Pins Setup
  pinMode(IN1_1, OUTPUT);
  pinMode(IN2_1, OUTPUT);
  pinMode(IN3_1, OUTPUT);
  pinMode(IN4_1, OUTPUT);
  pinMode(IN1_2, OUTPUT);
  pinMode(IN2_2, OUTPUT);
  pinMode(IN3_2, OUTPUT);
  pinMode(IN4_2, OUTPUT);

  // Ultrasonic Sensor Setup
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Servo Setup
  ultrasonicServo.attach(SERVO_PIN);
  ultrasonicServo.write(90);  // Center servo initially

  // IR Receiver Setup
  IrReceiver.begin(IR_RECEIVER_PIN, ENABLE_LED_FEEDBACK);  // Start IR receiver with feedback LED

  // Test motors
  testMotors();

  testRightFrontMotor();

  Serial.println("Setup complete.");
}

void loop() {
  // Check for obstacles first
  if (isObstacleDetected()) {
    Serial.println("Obstacle detected. Avoiding...");
    avoidObstacle();
  } else {
    // If no obstacle, adjust direction based on IR signals
    adjustDirection();
  }

  delay(50);  // Minimize delay to improve responsiveness
}

void testMotors() {
  digitalWrite(IN1_1, HIGH);
  digitalWrite(IN2_1, LOW);
  digitalWrite(IN3_1, HIGH);
  digitalWrite(IN4_1, LOW);

  digitalWrite(IN1_2, HIGH);
  digitalWrite(IN2_2, LOW);
  digitalWrite(IN3_2, HIGH);
  digitalWrite(IN4_2, LOW);
  delay(500);

  stopMotors();
}

void testRightFrontMotor() {
  digitalWrite(IN1_1, HIGH);   // Set direction to forward
  digitalWrite(IN2_1, LOW);
  digitalWrite(IN3_1, HIGH);   // Enable motor
  digitalWrite(IN4_1, LOW);

  delay(1000); // Run for 1 second
  
  digitalWrite(IN3_1, LOW);    // Stop motor
  digitalWrite(IN4_1, LOW);
}


bool isObstacleDetected() {
  int distance = measureDistance();
  if (distance == -1) return false;  // Ignore invalid distances
  
  Serial.print("Obstacle Distance: ");
  Serial.println(distance);

  return (distance <= OBSTACLE_DISTANCE);
}


int measureDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH, 30000); // Timeout in 30ms
  int distance = duration * 0.034 / 2; // Convert to cm
  Serial.print("Measured Distance: ");
  Serial.println(distance);

  if (distance <= 0 || distance > 400) {
    return -1;  // Invalid distance
  }
  return distance;
}

void avoidObstacle() {
  stopMotors();  // Explicitly stop motors
  moveBackward();
  delay(500); // Move back a little

  stopMotors();  // Ensure the robot stops before turning
  ultrasonicServo.write(45);
  delay(600);  // Wait for servo to stabilize

  int leftDistance = measureDistance();
  ultrasonicServo.write(135);
  delay(600);
  int rightDistance = measureDistance();

  ultrasonicServo.write(90);  // Center the servo
  delay(600);

  // Turn towards the side with more space
  if (leftDistance > rightDistance) {
    turnLeft();
    Serial.println("Turn left");
  } else {
    turnRight();
    Serial.println("Turn right");
  }

  // Move forward after turning
  moveForward();
  delay(1000);
  stopMotors();
}


void adjustDirection() {
  static unsigned long lastSignalTime = 0;  // Track time of last signal
  static bool isMoving = false;
  unsigned long irSignal = 0;  // Declare irSignal outside the if block

  if (IrReceiver.decode()) {  // If a signal is received
    irSignal = IrReceiver.decodedIRData.decodedRawData;

    if (irSignal == 0) {
      Serial.println("Invalid IR signal, ignoring...");
      IrReceiver.resume();
      return;
    }

    Serial.print("Received IR signal: ");
    Serial.println(irSignal, HEX);
    IrReceiver.resume();

    if (irSignal >= STRONG_SIGNAL_THRESHOLD) {
      // Strong signal: move forward
      moveForward();
      isMoving = true;
    } else if (irSignal < STRONG_SIGNAL_THRESHOLD && irSignal > WEAK_SIGNAL_THRESHOLD) {
      // Weak signal: adjust direction
      int speedAdjustment = map(irSignal, WEAK_SIGNAL_THRESHOLD, STRONG_SIGNAL_THRESHOLD, 50, 255);

      // Left motor (full speed)
      analogWrite(IN1_1, 255);
      analogWrite(IN2_1, 0);
      analogWrite(IN3_1, 255);
      analogWrite(IN4_1, 0);

      // Right motor (adjusted speed for weaker motor)
      analogWrite(IN1_2, 255);  // Full speed for left side
      analogWrite(IN2_2, 0);
      analogWrite(IN3_2, speedAdjustment);  // Right motor adjusted speed
      analogWrite(IN4_2, speedAdjustment);

      isMoving = true;
    } else {
      // Signal too weak or lost
      stopMotors();
      isMoving = false;
    }

    lastSignalTime = millis(); // Update the last signal time
  } else if (isMoving && millis() - lastSignalTime > 1000) {
    // Stop motors if no signal is received for 1 second
    stopMotors();
    isMoving = false;
  }
}


void moveForward() {
  if (isTurning) return;  // Don't move forward if the robot is turning
  digitalWrite(IN1_1, HIGH);
  digitalWrite(IN2_1, LOW);
  digitalWrite(IN3_1, HIGH);
  digitalWrite(IN4_1, LOW);

  digitalWrite(IN1_2, HIGH);
  digitalWrite(IN2_2, LOW);
  digitalWrite(IN3_2, HIGH);
  digitalWrite(IN4_2, LOW);

  Serial.print("Motor 1: ");
  Serial.print(digitalRead(IN1_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_1));
  
  Serial.print("Motor 2: ");
  Serial.print(digitalRead(IN3_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_1));
  
  Serial.print("Motor 3: ");
  Serial.print(digitalRead(IN1_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_2));
  
  Serial.print("Motor 4: ");
  Serial.print(digitalRead(IN3_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_2));

  Serial.println("Moving Forward");
}

void moveBackward() {
  digitalWrite(IN1_1, LOW);
  digitalWrite(IN2_1, HIGH);
  digitalWrite(IN3_1, LOW);
  digitalWrite(IN4_1, HIGH);

  digitalWrite(IN1_2, LOW);
  digitalWrite(IN2_2, HIGH);
  digitalWrite(IN3_2, LOW);
  digitalWrite(IN4_2, HIGH);

  Serial.print("Motor 1: ");
  Serial.print(digitalRead(IN1_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_1));
  
  Serial.print("Motor 2: ");
  Serial.print(digitalRead(IN3_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_1));
  
  Serial.print("Motor 3: ");
  Serial.print(digitalRead(IN1_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_2));
  
  Serial.print("Motor 4: ");
  Serial.print(digitalRead(IN3_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_2));

  Serial.println("Moving Backwards");
}

void turnLeft() {
  // Turning logic
  digitalWrite(IN1_1, LOW);
  digitalWrite(IN2_1, HIGH);
  digitalWrite(IN3_1, HIGH);
  digitalWrite(IN4_1, LOW);

  digitalWrite(IN1_2, LOW);
  digitalWrite(IN2_2, HIGH);
  digitalWrite(IN3_2, HIGH);
  digitalWrite(IN4_2, LOW);

  Serial.print("Motor 1: ");
  Serial.print(digitalRead(IN1_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_1));
  
  Serial.print("Motor 2: ");
  Serial.print(digitalRead(IN3_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_1));
  
  Serial.print("Motor 3: ");
  Serial.print(digitalRead(IN1_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_2));
  
  Serial.print("Motor 4: ");
  Serial.print(digitalRead(IN3_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_2));

  delay(500);  // Adjust turn duration based on needs
  stopMotors(); // Ensure turn ends cleanly
}

void turnRight() {
  // Turning logic
  digitalWrite(IN1_1, HIGH);
  digitalWrite(IN2_1, LOW);
  digitalWrite(IN3_1, LOW);
  digitalWrite(IN4_1, HIGH);

  digitalWrite(IN1_2, HIGH);
  digitalWrite(IN2_2, LOW);
  digitalWrite(IN3_2, LOW);
  digitalWrite(IN4_2, HIGH);

  Serial.print("Motor 1: ");
  Serial.print(digitalRead(IN1_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_1));
  
  Serial.print("Motor 2: ");
  Serial.print(digitalRead(IN3_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_1));
  
  Serial.print("Motor 3: ");
  Serial.print(digitalRead(IN1_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_2));
  
  Serial.print("Motor 4: ");
  Serial.print(digitalRead(IN3_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_2));


  delay(500);  // Adjust turn duration based on needs
  stopMotors(); // Ensure turn ends cleanly
}

void stopMotors() {
  digitalWrite(IN1_1, LOW);
  digitalWrite(IN2_1, LOW);
  digitalWrite(IN3_1, LOW);
  digitalWrite(IN4_1, LOW);

  digitalWrite(IN1_2, LOW);
  digitalWrite(IN2_2, LOW);
  digitalWrite(IN3_2, LOW);
  digitalWrite(IN4_2, LOW);

  Serial.println("Stopping Motors");
}

jim-p · February 1, 2025, 4:10pm

If it was made of metal, that could short it out.

superf1boi · February 2, 2025, 9:50pm

Do you have any suggestions on how to attach my sensor to the sensor holder?

TomGeorge · February 2, 2025, 9:54pm

Please post an image of the two devices.

Thanks... Tom....

superf1boi · February 2, 2025, 9:57pm

These photos are of my now damaged sensor on my holder.

superf1boi · February 3, 2025, 1:23am

Hi, I have another question. I'm having a strange issue where my Motor 3 is acting odd. It is moving forward delayed and isn't moving backwards. I don't think its a hardware issue. I used a different motor driver, and then the motor worked perfectly fine when I ran my test code. However, my main code is where it went wrong and I didn't change anything hardware-wise from running my test code to my main code. I also used a different motor to test. Does anyone know what could be the problem?

Main code (doesn't work):

#include <IRremote.hpp>
#include <Servo.h>

// Motor Driver Pins (L298N)
int IN1_1 = 6;  // Motor 1 direction control
int IN2_1 = 7;  // Motor 1 direction control
int IN3_1 = 8;  // Motor 2 direction control
int IN4_1 = 9;  // Motor 2 direction control

int IN1_2 = 12; // Motor 3 direction control
int IN2_2 = 13; // Motor 3 direction control
int IN3_2 = 4;  // Motor 4 direction control
int IN4_2 = 2;  // Motor 4 direction control


const int IR_RECEIVER_PIN = A2;  
const int TRIG_PIN = A0;        
const int ECHO_PIN = A1;         
const int SERVO_PIN = A3;         

// Obstacle Detection Threshold
const int OBSTACLE_DISTANCE = 15;  // Stop if an obstacle is within 15 cm

// Signal Thresholds
const int STRONG_SIGNAL_THRESHOLD = 1000;
const int WEAK_SIGNAL_THRESHOLD = 200;

// Servo setup
Servo ultrasonicServo;

// Flag to track turning state
bool isTurning = false;

// Function Prototypes
void testMotors();
bool isObstacleDetected();
int measureDistance();
void avoidObstacle();
void adjustDirection();
void moveForward();
void moveBackward();
void turnLeft();
void turnRight();
void stopMotors();

void setup() {
  Serial.begin(9600);
  Serial.println("Initializing...");

  // Motor Driver Pins Setup
  pinMode(IN1_1, OUTPUT);
  pinMode(IN2_1, OUTPUT);
  pinMode(IN3_1, OUTPUT);
  pinMode(IN4_1, OUTPUT);
  pinMode(IN1_2, OUTPUT);
  pinMode(IN2_2, OUTPUT);
  pinMode(IN3_2, OUTPUT);
  pinMode(IN4_2, OUTPUT);

  // Ultrasonic Sensor Setup
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Servo Setup
  ultrasonicServo.attach(SERVO_PIN);
  ultrasonicServo.write(90);  // Center servo initially

  // IR Receiver Setup
  IrReceiver.begin(IR_RECEIVER_PIN, ENABLE_LED_FEEDBACK);  // Start IR receiver with feedback LED

  // Test motors
  testMotors();

  testRightFrontMotor();

  Serial.println("Setup complete.");
}

void loop() {
  // Check for obstacles first
  if (isObstacleDetected()) {
    Serial.println("Obstacle detected. Avoiding...");
    avoidObstacle();
  } else {
    // If no obstacle, adjust direction based on IR signals
    adjustDirection();
  }

  delay(50);  // Minimize delay to improve responsiveness
}

void testMotors() {
  digitalWrite(IN1_1, HIGH);
  digitalWrite(IN2_1, LOW);
  digitalWrite(IN3_1, HIGH);
  digitalWrite(IN4_1, LOW);

  digitalWrite(IN1_2, HIGH);
  digitalWrite(IN2_2, LOW);
  digitalWrite(IN3_2, HIGH);
  digitalWrite(IN4_2, LOW);
  delay(500);

  stopMotors();
}

void testRightFrontMotor() {
  digitalWrite(IN1_1, HIGH);   // Set direction to forward
  digitalWrite(IN2_1, LOW);
  digitalWrite(IN3_1, HIGH);   // Enable motor
  digitalWrite(IN4_1, LOW);

  delay(1000); // Run for 1 second
  
  digitalWrite(IN3_1, LOW);    // Stop motor
  digitalWrite(IN4_1, LOW);
}


bool isObstacleDetected() {
  int distance = measureDistance();
  if (distance == -1) return false;  // Ignore invalid distances
  
  Serial.print("Obstacle Distance: ");
  Serial.println(distance);

  return (distance <= OBSTACLE_DISTANCE);
}


int measureDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH, 30000); // Timeout in 30ms
  int distance = duration * 0.034 / 2; // Convert to cm
  Serial.print("Measured Distance: ");
  Serial.println(distance);

  if (distance <= 0 || distance > 400) {
    return -1;  // Invalid distance
  }
  return distance;
}

void avoidObstacle() {
  stopMotors();  // Explicitly stop motors
  moveBackward();
  delay(500); // Move back a little

  stopMotors();  // Ensure the robot stops before turning
  ultrasonicServo.write(45);
  delay(600);  // Wait for servo to stabilize

  int leftDistance = measureDistance();
  ultrasonicServo.write(135);
  delay(600);
  int rightDistance = measureDistance();

  ultrasonicServo.write(90);  // Center the servo
  delay(600);

  // Turn towards the side with more space
  if (leftDistance > rightDistance) {
    turnLeft();
    Serial.println("Turn left");
  } else {
    turnRight();
    Serial.println("Turn right");
  }

  // Move forward after turning
  moveForward();
  delay(1000);
  stopMotors();
}


void adjustDirection() {
  static unsigned long lastSignalTime = 0;  // Track time of last signal
  static bool isMoving = false;
  unsigned long irSignal = 0;  // Declare irSignal outside the if block

  if (IrReceiver.decode()) {  // If a signal is received
    irSignal = IrReceiver.decodedIRData.decodedRawData;

    if (irSignal == 0) {
      Serial.println("Invalid IR signal, ignoring...");
      IrReceiver.resume();
      return;
    }

    Serial.print("Received IR signal: ");
    Serial.println(irSignal, HEX);
    IrReceiver.resume();

    if (irSignal >= STRONG_SIGNAL_THRESHOLD) {
      // Strong signal: move forward
      moveForward();
      isMoving = true;
    } else if (irSignal < STRONG_SIGNAL_THRESHOLD && irSignal > WEAK_SIGNAL_THRESHOLD) {
      // Weak signal: adjust direction
      int speedAdjustment = map(irSignal, WEAK_SIGNAL_THRESHOLD, STRONG_SIGNAL_THRESHOLD, 50, 255);

      // Left motor (full speed)
      digitalWrite(IN1_1, HIGH);
      digitalWrite(IN2_1, LOW);
      digitalWrite(IN3_1, HIGH);
      digitalWrite(IN4_1, LOW);

      // Right motor (adjusted speed for weaker motor)
      digitalWrite(IN1_2, HIGH);
      digitalWrite(IN2_2, LOW);
      digitalWrite(IN3_2, HIGH);
      digitalWrite(IN4_2, LOW);


      isMoving = true;
    } else {
      // Signal too weak or lost
      stopMotors();
      isMoving = false;
    }

    lastSignalTime = millis(); // Update the last signal time
  } else if (isMoving && millis() - lastSignalTime > 1000) {
    // Stop motors if no signal is received for 1 second
    stopMotors();
    isMoving = false;
  }
}


void moveForward() {
  if (isTurning) return;  // Don't move forward if the robot is turning
  digitalWrite(IN1_1, HIGH);
  digitalWrite(IN2_1, LOW);
  digitalWrite(IN3_1, HIGH);
  digitalWrite(IN4_1, LOW);

  digitalWrite(IN1_2, HIGH);
  digitalWrite(IN2_2, LOW);
  digitalWrite(IN3_2, HIGH);
  digitalWrite(IN4_2, LOW);

  Serial.print("Motor 1: ");
  Serial.print(digitalRead(IN1_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_1));
  
  Serial.print("Motor 2: ");
  Serial.print(digitalRead(IN3_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_1));
  
  Serial.print("Motor 3: ");
  Serial.print(digitalRead(IN1_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_2));
  
  Serial.print("Motor 4: ");
  Serial.print(digitalRead(IN3_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_2));

  Serial.println("Moving Forward");
}

void moveBackward() {
  digitalWrite(IN1_1, LOW);
  digitalWrite(IN2_1, HIGH);
  digitalWrite(IN3_1, LOW);
  digitalWrite(IN4_1, HIGH);

  digitalWrite(IN1_2, LOW);
  digitalWrite(IN2_2, HIGH);
  digitalWrite(IN3_2, LOW);
  digitalWrite(IN4_2, HIGH);

  Serial.print("Motor 1: ");
  Serial.print(digitalRead(IN1_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_1));
  
  Serial.print("Motor 2: ");
  Serial.print(digitalRead(IN3_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_1));
  
  Serial.print("Motor 3: ");
  Serial.print(digitalRead(IN1_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_2));
  
  Serial.print("Motor 4: ");
  Serial.print(digitalRead(IN3_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_2));

  Serial.println("Moving Backwards");
}

void turnLeft() {
  // Turning logic
  digitalWrite(IN1_1, LOW);
  digitalWrite(IN2_1, HIGH);
  digitalWrite(IN3_1, HIGH);
  digitalWrite(IN4_1, LOW);

  digitalWrite(IN1_2, LOW);
  digitalWrite(IN2_2, HIGH);
  digitalWrite(IN3_2, HIGH);
  digitalWrite(IN4_2, LOW);

  Serial.print("Motor 1: ");
  Serial.print(digitalRead(IN1_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_1));
  
  Serial.print("Motor 2: ");
  Serial.print(digitalRead(IN3_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_1));
  
  Serial.print("Motor 3: ");
  Serial.print(digitalRead(IN1_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_2));
  
  Serial.print("Motor 4: ");
  Serial.print(digitalRead(IN3_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_2));

  delay(500);  // Adjust turn duration based on needs
  stopMotors(); // Ensure turn ends cleanly
}

void turnRight() {
  // Turning logic
  digitalWrite(IN1_1, HIGH);
  digitalWrite(IN2_1, LOW);
  digitalWrite(IN3_1, LOW);
  digitalWrite(IN4_1, HIGH);

  digitalWrite(IN1_2, HIGH);
  digitalWrite(IN2_2, LOW);
  digitalWrite(IN3_2, LOW);
  digitalWrite(IN4_2, HIGH);

  Serial.print("Motor 1: ");
  Serial.print(digitalRead(IN1_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_1));
  
  Serial.print("Motor 2: ");
  Serial.print(digitalRead(IN3_1));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_1));
  
  Serial.print("Motor 3: ");
  Serial.print(digitalRead(IN1_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN2_2));
  
  Serial.print("Motor 4: ");
  Serial.print(digitalRead(IN3_2));
  Serial.print(", ");
  Serial.println(digitalRead(IN4_2));


  delay(500);  // Adjust turn duration based on needs
  stopMotors(); // Ensure turn ends cleanly
}

void stopMotors() {
  digitalWrite(IN1_1, LOW);
  digitalWrite(IN2_1, LOW);
  digitalWrite(IN3_1, LOW);
  digitalWrite(IN4_1, LOW);

  digitalWrite(IN1_2, LOW);
  digitalWrite(IN2_2, LOW);
  digitalWrite(IN3_2, LOW);
  digitalWrite(IN4_2, LOW);

  Serial.println("Stopping Motors");
}

Test code (works):

// Motor Driver 1
int IN1_1 = 6;  // Motor 1 direction control
int IN2_1 = 7;  // Motor 1 direction control
int IN3_1 = 8;  // Motor 2 direction control
int IN4_1 = 9;  // Motor 2 direction control

// Motor Driver 2
int IN1_2 = 12; // Motor 3 direction control
int IN2_2 = 13; // Motor 3 direction control
int IN3_2 = 4;  // Motor 4 direction control
int IN4_2 = 2;  // Motor 4 direction control

// Ultrasonic Sensor
int trigPin = A0; // D14 (A0) - Trigger pin
int echoPin = A1; // D15 (A1) - Echo pin

// IR Sensor
int irSensorPin = A2; // D16 (A2) - IR sensor output pin

// Servo Motor
int servoPin = A3; // D17 (A3) - Servo motor control pin

void setup() {
  // Set motor control pins as output
  pinMode(IN1_1, OUTPUT);
  pinMode(IN2_1, OUTPUT);
  pinMode(IN3_1, OUTPUT);
  pinMode(IN4_1, OUTPUT);

  pinMode(IN1_2, OUTPUT);
  pinMode(IN2_2, OUTPUT);
  pinMode(IN3_2, OUTPUT);
  pinMode(IN4_2, OUTPUT);

  // Set sensor pins as input/output
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(irSensorPin, INPUT);

  // Set servo pin
  pinMode(servoPin, OUTPUT);

  // Initialize serial communication for debugging
  Serial.begin(9600);
}

void loop() {
  // Motor Test: Rotate motors in one direction (FORWARD)
  Serial.println("Motors Moving FORWARD");

  digitalWrite(IN1_1, HIGH);
  digitalWrite(IN2_1, LOW);
  digitalWrite(IN3_1, HIGH);
  digitalWrite(IN4_1, LOW);

  digitalWrite(IN1_2, HIGH);
  digitalWrite(IN2_2, LOW);
  digitalWrite(IN3_2, HIGH);
  digitalWrite(IN4_2, LOW);

  delay(2000);  // Run motors for 2 seconds

  // Stop motors
  Serial.println("Motors STOPPED");
  
  digitalWrite(IN1_1, LOW);
  digitalWrite(IN2_1, LOW);
  digitalWrite(IN3_1, LOW);
  digitalWrite(IN4_1, LOW);

  digitalWrite(IN1_2, LOW);
  digitalWrite(IN2_2, LOW);
  digitalWrite(IN3_2, LOW);
  digitalWrite(IN4_2, LOW);

  delay(1000);  // Wait for 1 second before reversing

  // Motor Test: Reverse motors (REVERSE)
  Serial.println("Motors Moving REVERSE");

  digitalWrite(IN1_1, LOW);
  digitalWrite(IN2_1, HIGH);
  digitalWrite(IN3_1, LOW);
  digitalWrite(IN4_1, HIGH);

  digitalWrite(IN1_2, LOW);
  digitalWrite(IN2_2, HIGH);
  digitalWrite(IN3_2, LOW);
  digitalWrite(IN4_2, HIGH);

  delay(2000);  // Run motors for 2 seconds

  // Stop motors
  Serial.println("Motors STOPPED");

  digitalWrite(IN1_1, LOW);
  digitalWrite(IN2_1, LOW);
  digitalWrite(IN3_1, LOW);
  digitalWrite(IN4_1, LOW);

  digitalWrite(IN1_2, LOW);
  digitalWrite(IN2_2, LOW);
  digitalWrite(IN3_2, LOW);
  digitalWrite(IN4_2, LOW);

  delay(1000);  // Wait for 1 second before next loop
}

When it writes the IN pin digital values, the right front motor's IN1 and IN2 are both 0, meaning that Motor 3 is getting overridden by something as in the moveBackwards() function, it is set to 0,1.

jim-p · February 3, 2025, 8:51am

Looks OK to me.

jim-p · February 3, 2025, 9:05am

Well you have posted several different wiring diagrams and none match your code.

GoForSmoke · February 3, 2025, 9:22am

I see a lot of places where timing is okay to do one thing but not coordinate multiple actions.

From experience I also know it's > 90% likely there's no easy way to get past that short of writing what will work with very small chance of seeing it tried much less tuned.

superf1boi · February 7, 2025, 1:37am

Thanks for the suggestion! I'm not sure how to fix the timing of my code. The motor can work individually in the setup, but then it can't work with the other 3 motors (those motors work perfectly fine). Do you have an idea on how I can improve the timing or solve this issue?

#include <IRremote.hpp>
#include <Servo.h>

// PWM pins for motor speed control
int ENA1 = 10;
int ENB1 = 11;
int ENA2 = 9;
int ENB2 = 5;

// Motor Driver 1
int IN1_1 = 4;  // Motor 1 direction control
int IN2_1 = 7;  
int IN3_1 = 8;  // Motor 2 direction control
int IN4_1 = 6;  

// Motor Driver 2
int IN1_2 = 12; // Motor 3 direction control
int IN2_2 = 13; 
int IN3_2 = 3;  // Motor 4 direction control
int IN4_2 = 2;

const int IR_RECEIVER_PIN = A2;  
const int TRIG_PIN = A0;        
const int ECHO_PIN = A1;         
const int SERVO_PIN = A3;         

// Obstacle Detection Threshold
const int OBSTACLE_DISTANCE = 15;  // Stop if an obstacle is within 15 cm

// Signal Thresholds
const int STRONG_SIGNAL_THRESHOLD = 1000;
const int WEAK_SIGNAL_THRESHOLD = 200;

// Servo setup
Servo ultrasonicServo;

// Flag to track turning state
bool isTurning = false;

// Function Prototypes
void testMotors();
bool isObstacleDetected();
int measureDistance();
void avoidObstacle();
void adjustDirection();
void moveForward();
void moveBackward();
void turnLeft();
void turnRight();
void stopMotors();
void setMotorSpeed(int speed, bool forward);

void setup() {
  Serial.begin(9600);
  Serial.println("Initializing...");

  // Motor Driver Pins Setup
  pinMode(IN1_1, OUTPUT);
  pinMode(IN2_1, OUTPUT);
  pinMode(IN3_1, OUTPUT);
  pinMode(IN4_1, OUTPUT);
  pinMode(IN1_2, OUTPUT);
  pinMode(IN2_2, OUTPUT);
  pinMode(IN3_2, OUTPUT);
  pinMode(IN4_2, OUTPUT);

  // PWM Pins Setup
  pinMode(ENA1, OUTPUT);
  pinMode(ENB1, OUTPUT);
  pinMode(ENA2, OUTPUT);
  pinMode(ENB2, OUTPUT);

  // Ultrasonic Sensor Setup
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Servo Setup
  ultrasonicServo.attach(SERVO_PIN);
  ultrasonicServo.write(90);  // Center servo 
    
  // Test Forward
  Serial.println("Forward Test");
  digitalWrite(IN1_2, HIGH);
  digitalWrite(IN2_2, LOW);
  analogWrite(ENA2, 255);
  delay(100);
    
  // Test Backward
  Serial.println("Backward Test");
  digitalWrite(IN1_2, LOW);
  digitalWrite(IN2_2, HIGH);
  analogWrite(ENA2, 255);
  delay(100);
    
  Serial.println("Stopping Motor");
  digitalWrite(IN1_2, LOW);
  digitalWrite(IN2_2, LOW);
  analogWrite(ENA2, 0);

  testMotors();
  testRightFrontMotor();

  IrReceiver.begin(IR_RECEIVER_PIN, ENABLE_LED_FEEDBACK);  // Start IR receiver with feedback 
  Serial.println("Setup complete.");
}

unsigned long lastMoveTime = 0;
const int moveInterval = 500; // Milliseconds between movements

void loop() {
  if (millis() - lastMoveTime > moveInterval) {
    lastMoveTime = millis();  // Update time of last move
    if (isObstacleDetected()) {
      avoidObstacle();
    } else {
      adjustDirection();
    }
  }
}


void testMotors() {
  setMotorSpeed(255, true); // Set speed to full and move forward
  delay(500);
  stopMotors();
}

void testRightFrontMotor() {
  Serial.println("Testing Motor 3 FORWARD...");
  Serial.print("Before FORWARD: IN1_2 = ");
  Serial.println(digitalRead(IN1_2));
  Serial.print("Before FORWARD: IN2_2 = ");
  Serial.println(digitalRead(IN2_2));
  digitalWrite(IN1_2, HIGH);  // Motor 3 forward
  digitalWrite(IN2_2, LOW);
  analogWrite(ENA2, 255);     // Max speed
  delay(2000);
  Serial.print("After FORWARD: IN1_2 = ");
  Serial.println(digitalRead(IN1_2));
  Serial.print("After FORWARD: IN2_2 = ");
  Serial.println(digitalRead(IN2_2));
  delay(2000);
  
  Serial.println("Testing Motor 3 BACKWARD...");
  digitalWrite(IN1_2, LOW);
  digitalWrite(IN2_2, HIGH);
  delay(10);  // Small delay for stability
  analogWrite(ENA2, 255);     // Max speed
  delay(2000);
  Serial.print("After BACKWARD: IN1_2 = ");
  Serial.println(digitalRead(IN1_2));
  Serial.print("After BACKWARD: IN2_2 = ");
  Serial.println(digitalRead(IN2_2));
  delay(2000);
  
  Serial.println("Stopping Motor 3...");
  digitalWrite(IN1_2, LOW);
  digitalWrite(IN2_2, LOW);
  analogWrite(ENA2, 0);  
  delay(2000);
}

bool isObstacleDetected() {
  int distance = measureDistance();
  if (distance == -1) return false;
  Serial.print("Obstacle Distance: ");
  Serial.println(distance);
  return (distance <= OBSTACLE_DISTANCE);
}

int measureDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long duration = pulseIn(ECHO_PIN, HIGH, 30000);
  int distance = duration * 0.034 / 2;
  Serial.print("Measured Distance: ");
  Serial.println(distance);
  if (distance <= 0 || distance > 400) return -1;
  return distance;
}

void avoidObstacle() {
  stopMotors();
  Serial.print("Motor 3 - IN1_2: "); Serial.println(digitalRead(IN1_2));
  Serial.print("Motor 3 - IN2_2: "); Serial.println(digitalRead(IN2_2));
  moveBackward();
  Serial.print("Motor 3 - IN1_2: "); Serial.println(digitalRead(IN1_2));
  Serial.print("Motor 3 - IN2_2: "); Serial.println(digitalRead(IN2_2));
  delay(500);
  stopMotors();
  ultrasonicServo.write(45);
  delay(600);
  int leftDistance = measureDistance();
  ultrasonicServo.write(135);
  delay(600);
  int rightDistance = measureDistance();
  ultrasonicServo.write(90);
  delay(600);
  if (leftDistance > rightDistance) {
    turnLeft();
    Serial.println("Turn left");
  } else {
    turnRight();
    Serial.println("Turn right");
  }
  moveForward();
  delay(1000);
  stopMotors();
}

void adjustDirection() {
  static unsigned long lastSignalTime = 0;
  static bool isMoving = false;
  unsigned long irSignal = 0;
  if (IrReceiver.decode()) {
    irSignal = IrReceiver.decodedIRData.decodedRawData;
    if (irSignal == 0) {
      Serial.println("Invalid IR signal, ignoring...");
      IrReceiver.resume();
      return;
    }
    Serial.print("Received IR signal: ");
    Serial.println(irSignal, HEX);
    IrReceiver.resume();
    if (irSignal >= STRONG_SIGNAL_THRESHOLD) {
      moveForward();
      isMoving = true;
    } else if (irSignal < STRONG_SIGNAL_THRESHOLD && irSignal > WEAK_SIGNAL_THRESHOLD) {
      int speedAdjustment = map(irSignal, WEAK_SIGNAL_THRESHOLD, STRONG_SIGNAL_THRESHOLD, 50, 255);
      setMotorSpeed(speedAdjustment, true);
      isMoving = true;
    } else {
      stopMotors();
      isMoving = false;
    }
    lastSignalTime = millis();
  } else if (isMoving && millis() - lastSignalTime > 1000) {
    stopMotors();
    isMoving = false;
  }
}

void moveForward() {
  if (isTurning) return;
  setMotorSpeed(255, true);
  Serial.println("Moving Forward");
}

void moveBackward() {
  setMotorSpeed(255, false);
  Serial.println("Moving Backward");
}

// Differential turn functions for pivoting
void turnLeft() {
  Serial.println("Turning Left (Differential)");
  // Left motors: backward; Right motors: forward
  // Left Motors (M1 & M2)
  digitalWrite(IN1_1, LOW);
  digitalWrite(IN2_1, HIGH);
  digitalWrite(IN3_1, LOW);
  digitalWrite(IN4_1, HIGH);
  analogWrite(ENA1, 255);
  analogWrite(ENB1, 255);
  // Right Motors (M3 & M4)
  digitalWrite(IN1_2, HIGH);
  digitalWrite(IN2_2, LOW);
  digitalWrite(IN3_2, HIGH);
  digitalWrite(IN4_2, LOW);
  analogWrite(ENA2, 255);
  analogWrite(ENB2, 255);
}

void turnRight() {
  Serial.println("Turning Right (Differential)");
  // Left motors: forward; Right motors: backward
  // Left Motors (M1 & M2)
  digitalWrite(IN1_1, HIGH);
  digitalWrite(IN2_1, LOW);
  digitalWrite(IN3_1, HIGH);
  digitalWrite(IN4_1, LOW);
  analogWrite(ENA1, 255);
  analogWrite(ENB1, 255);
  // Right Motors (M3 & M4)
  digitalWrite(IN1_2, LOW);
  digitalWrite(IN2_2, HIGH);
  digitalWrite(IN3_2, LOW);
  digitalWrite(IN4_2, HIGH);
  analogWrite(ENA2, 255);
  analogWrite(ENB2, 255);
}

void stopMotors() {
  analogWrite(ENA1, 0);
  analogWrite(ENB1, 0);
  analogWrite(ENA2, 0);
  analogWrite(ENB2, 0);
  Serial.println("Motors Stopped");
}

void setMotorSpeed(int speed, bool forward) {
  if (forward) {
    // Forward movement: both motors 1 & 2 (for M1, M2, M3, M4)
    digitalWrite(IN1_1, HIGH);
    digitalWrite(IN2_1, LOW);
    digitalWrite(IN3_1, HIGH);
    digitalWrite(IN4_1, LOW);
    analogWrite(ENA1, speed);
    analogWrite(ENB1, speed);

    digitalWrite(IN1_2, HIGH);  // Forward for Motor 3
    digitalWrite(IN2_2, LOW);
    digitalWrite(IN3_2, HIGH);  // Forward for Motor 4
    digitalWrite(IN4_2, LOW);
    analogWrite(ENA2, speed);
    analogWrite(ENB2, speed);
  } else {
    // Backward movement: both motors 1 & 2 (for M1, M2, M3, M4)
    digitalWrite(IN1_1, LOW);
    digitalWrite(IN2_1, HIGH);
    digitalWrite(IN3_1, LOW);
    digitalWrite(IN4_1, HIGH);
    analogWrite(ENA1, speed);
    analogWrite(ENB1, speed);

    digitalWrite(IN1_2, LOW);  // Backward for Motor 3
    digitalWrite(IN2_2, HIGH);
    digitalWrite(IN3_2, LOW);  // Backward for Motor 4
    digitalWrite(IN4_2, HIGH);
    analogWrite(ENA2, speed);
    analogWrite(ENB2, speed);
  }
}

Thank you!

GoForSmoke · February 7, 2025, 4:19am

You need to get rid of those frikking delay() calls.

How to do more than one thing at a time, 101 Intro tutorial.

That is not going to fix everything for you but it should get you started in the right direction. Your tasks are to run ultrasonic ping and multiple motors at the same time without stepping on each other.
FIRST you learn what is called "millis code" to get rid of "delay code", THEN you learn about "state machines" and from there you apply those to what you're doing.

You can spend many hours on that to know what to do --OR-- you can sink as long as you want down the path you've been on. I'm not sticking around for the latter choice.

Topic		Replies	Views
problems with robot using ultrasonic sensor to avoid obstacles [solved] Robotics	25	8921	May 6, 2021
I have a problem with 2 of my 4 ultrasonic sensor's Motors, Mechanics, Power and CNC	22	241	October 30, 2024
Arduino blocks ultrasonic sensor Programming	38	2319	November 18, 2023
Issue with Ultrasonic sensor code, Sensors	20	3534	September 25, 2022
please give me programming code if you have project with this components General Guidance	41	9262	May 5, 2021

Ultrasonic sensor not measuring distance

Related topics