Complete Arduino Code for Robot Navigation: Uno R4 & TB6612FNG Build

The Hardware Decision: Which Motor Driver to Pick

Before writing a single line of code, you need to make a hardware decision that dictates your robot's battery life and thermal management. Most legacy tutorials default to the L298N motor driver. This is a mistake for modern builds. The L298N uses bipolar junction transistors (BJTs), which drop roughly 1.5V to 2V across the chip as heat. If you feed it 6V from a 4xAA pack, your motors only see 4V, resulting in sluggish torque and stalled microcontrollers.

Use the decision tree below to select the correct driver for your specific chassis and power supply.

Exact Parts List and Pin Mapping

This build targets the Arduino Uno R4 Minima. The R4 features a 32-bit ARM Cortex-M4 processor, which handles the ultrasonic timing and PWM generation much more cleanly than the older 8-bit R3, though the code below is fully backward-compatible with the Uno R3.

Bill of Materials (BOM)

• Microcontroller: Arduino Uno R4 Minima (or Uno R3)
• Motor Driver: TB6612FNG Dual Motor Driver Breakout
• Sensor: HC-SR04 Ultrasonic Distance Sensor
• Chassis: Generic 2WD Acrylic Chassis with 130-size TT motors (1:48 gear ratio)
• Power: 2S LiPo Battery (7.4V, 1000mAh) with XT60 connector, or 4x AA Holder (6V)
• Regulator: LM2596 Buck Converter (stepped down to 5V for the Arduino VIN pin)

Pin Mapping Table

Wire the components exactly as shown. Do not share the 5V logic rail from the Arduino to power the motors; use the buck converter or a dedicated 5V BEC.

The Arduino Code for Robot Navigation

The following C++ code uses the NewPing library. Unlike the default pulseIn() function, NewPing handles sensor timeouts gracefully without blocking the main loop for 1000ms when the sensor reads empty space. This keeps your robot's steering responsive.

Prerequisite: Install the 'NewPing' library by Tim Eckel via the Arduino Library Manager before compiling.

#include <NewPing.h>

// --- PIN DEFINITIONS ---
// Motor A (Right)
const int AIN1 = 4;
const int AIN2 = 7;
const int PWMA = 5;

// Motor B (Left)
const int BIN1 = 9;
const int BIN2 = 10;
const int PWMB = 6;

// TB6612FNG Standby Pin
const int STBY = 8;

// HC-SR04 Ultrasonic Sensor
const int TRIG_PIN = 2;
const int ECHO_PIN = 3;
const int MAX_DISTANCE = 200; // Maximum distance we want to ping (in cm)

// Initialize NewPing object
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

// Navigation Parameters
const int OBSTACLE_THRESHOLD = 25; // Distance in cm to trigger a turn
const int BASE_SPEED = 180;        // PWM value (0-255)
const int TURN_SPEED = 150;        // PWM value for turning
const int PING_INTERVAL = 35;      // Milliseconds between pings (min 29ms)

unsigned long lastPingTime = 0;
unsigned int currentDistance = 0;

void setup() {
  Serial.begin(115200);
  
  // Configure motor pins as outputs
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(STBY, OUTPUT);

  // Enable the TB6612FNG driver
  digitalWrite(STBY, HIGH);
  
  Serial.println("Robot initialized. Starting navigation...");
}

void loop() {
  // Non-blocking ping sequence
  if (millis() - lastPingTime >= PING_INTERVAL) {
    lastPingTime = millis();
    
    // Get ping time in microseconds, convert to cm
    // If ping fails (out of range), sonar.ping_cm() returns 0
    currentDistance = sonar.ping_cm();
    
    // Fallback for max distance (NewPing returns 0 for > MAX_DISTANCE)
    if (currentDistance == 0) {
      currentDistance = MAX_DISTANCE;
    }
    
    Serial.print("Distance: ");
    Serial.print(currentDistance);
    Serial.println(" cm");
  }

  // Decision Logic
  if (currentDistance > OBSTACLE_THRESHOLD) {
    moveForward(BASE_SPEED);
  } else {
    // Obstacle detected: Stop, reverse slightly, then turn
    stopMotors();
    delay(100);
    moveBackward(BASE_SPEED);
    delay(300);
    stopMotors();
    
    // Decide turn direction (simple alternating or random)
    turnRight(TURN_SPEED);
    delay(400); // Roughly 90 degrees depending on battery voltage
    stopMotors();
  }
}

// --- MOTOR CONTROL FUNCTIONS ---

void moveForward(int speed) {
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  analogWrite(PWMA, speed);

  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMB, speed);
}

void moveBackward(int speed) {
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  analogWrite(PWMA, speed);

  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, HIGH);
  analogWrite(PWMB, speed);
}

void turnRight(int speed) {
  // Right motor backward, Left motor forward
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  analogWrite(PWMA, speed);

  digitalWrite(BIN1, HIGH);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMB, speed);
}

void stopMotors() {
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, LOW);
  analogWrite(PWMA, 0);

  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, LOW);
  analogWrite(PWMB, 0);
}

Debugging: First Three Things to Check When It Fails

When your robot refuses to move, or the code throws an error, do not start rewriting logic. Hardware integration is almost always the culprit. Here is the exact decision path for troubleshooting.

Compile Error: Missing Library

If the IDE throws this exact error string:

fatal error: NewPing.h: No such file or directory
compilation terminated.
exit status 1

Cause: The NewPing library is not installed, or you downloaded the ZIP and placed it in the wrong folder.
Fix: Go to Sketch > Include Library > Manage Libraries. Search for 'NewPing' by Tim Eckel and click Install. Restart the IDE.

The 'First Three' Hardware Checks

If the code compiles and uploads, but the robot sits dead on the bench, check these three things in order:

1. The STBY Pin on the TB6612FNG: This is the #1 beginner mistake. The TB6612FNG has a hardware standby pin. If it is floating or pulled LOW, the H-bridges are physically disabled. Verify with your multimeter that pin D8 is outputting 5V, and that the STBY pin on the breakout reads >4.5V.
2. Common Ground: The Arduino GND, the TB6612FNG GND, and the HC-SR04 GND must all be tied together. If the motor driver ground is only connected to the battery, the 5V logic signals from the Arduino have no reference voltage and the driver will ignore them.
3. Ultrasonic Echo/Trigger Swap: The HC-SR04 pins are often mislabeled on cheap clones. If your serial monitor prints '0 cm' constantly regardless of what you put in front of the sensor, swap the wires on D2 and D3. Measure the voltage on the VCC pin of the sensor; if it's below 4.5V, the sensor will fail to trigger the acoustic burst.

Runtime Brownouts (Arduino Resets)

Symptom: The robot moves forward, hits an obstacle, tries to reverse, and the Arduino's onboard 'L' LED flashes as the board reboots.
Cause: Motor stall current is pulling the battery voltage below the Arduino's brownout detection threshold (typically ~2.7V on the 5V rail).
Fix: Never power the Arduino 5V pin directly from the motor driver's logic out. Use a dedicated buck converter (like the LM2596) wired directly to the battery terminals, set to exactly 5.0V, and feed that into the Arduino's 5V pin (bypassing the onboard regulator).

Extending or Simplifying the Build

Once the base navigation loop is proven, you need to decide how to adapt the robot for your specific environment.

How to Simplify (For Younger Makers or Quick Prototypes)

Drop the HC-SR04 entirely. Ultrasonic sensors struggle with angled walls and sound-absorbing materials like curtains. Replace it with two mechanical limit switches (bumper switches) wired to digital inputs with internal pull-ups enabled (INPUT_PULLUP). Change the logic to drive forward until a switch reads LOW, then reverse and turn. This eliminates all timing libraries and reduces the code to basic digitalRead() state checks.

How to Extend (For Advanced Navigation)

If you need the robot to navigate glass doors or dark environments where optical sensors fail, swap the HC-SR04 for an RCWL-0516 Microwave Radar module. It detects motion and presence up to 7 meters away through non-metallic barriers. Alternatively, add an MPU6050 IMU via I2C (pins A4/A5 on the Uno R4). By integrating the Z-axis gyroscope data over time, you can replace the blind delay(400) turn with a precise 90-degree closed-loop PID turn, vastly improving dead-reckoning accuracy in maze-solving competitions.