Wall-following robot with ToF
Build it step by step
0 of 7 doneRead both sensors
Print distances.
Set the addresses
Use the XSHUT sequence.
Test the motors
Forward and back.
Follow the wall
Proportional steering only.
Add the derivative term
Stops the wobble.
Add the front turn
Right turn at corners.
Tune on a test track
Cardboard walls.
How do you build a wall-following robot with ToF?
A small robot keeps a fixed distance from a wall using side ToF sensors and PID. You need Arduino Nano, VL53L0X time-of-flight distance sensor × 2, TB6612FNG Motor Driver, BO motor straight 100 RPM × 2, Robot car chassis kit 2WD (acrylic), Battery holder 4 × AA with switch and AA alkaline battery × 4. It takes 7 steps and about 150 minutes.
- Level
- Intermediate
- Time
- About 150 min
- Parts
- 7
Wiring
| From | To | Why |
|---|---|---|
| Side sensor SDA, SCL | Nano A4 and A5 | Shared I2C bus |
| Front sensor SDA, SCL | Nano A4 and A5 | Shared I2C bus |
| Side sensor XSHUT | Nano D2 | Address setup |
| Front sensor XSHUT | Nano D3 | Address setup |
| TB6612 PWMA, AIN1, AIN2 | Nano D5, D7, D8 | Left motor |
| TB6612 PWMB, BIN1, BIN2 | Nano D6, D9, D10 | Right motor |
| TB6612 STBY | Nano D11 | Driver enable |
| Battery pack plus | TB6612 VM and Nano VIN | 6 V supply |
Code
Both VL53L0X sensors start with the same I2C address, so the sketch holds both in reset with their XSHUT pins, wakes the side sensor first and moves it to address 0x30, then wakes the front one at the default address. The controller measures how far the wall is from the target, adds a proportional push and a push from the change since last time, which steadies the steering, and turns the wheels in different amounts. Tune KP first with KD at zero until the robot follows the wall but wobbles, then raise KD until the wobble stops. If it steers away from the wall instead of toward it, swap the two signs on the last line. When the front sensor sees a wall ahead, the robot turns right on the spot. Test on a smooth wall with the robot on a table edge guard first, and keep fingers clear of the wheels. Four AA cells give 6 V for the motors. Arduino IDE, board "Arduino Nano" (try the Old Bootloader option if the upload fails).
Runs on a virtual Arduino Nano in your browser, with sliders for the sensors. Happy with it? Verify and upload to your real board from the same page (Chrome or Edge on a computer).
// Wall follower: keep the left wall at 150 mm with a PD controller. Libraries: Adafruit_VL53L0X. Board: Arduino Nano.
#include <Wire.h>
#include <Adafruit_VL53L0X.h>
Adafruit_VL53L0X side, front;
const int XS_SIDE = 2, XS_FRONT = 3;
const int PWMA = 5, AIN1 = 7, AIN2 = 8, PWMB = 6, BIN1 = 9, BIN2 = 10, STBY = 11;
const int TARGET = 150; // millimetres from the wall
const float KP = 0.8, KD = 2.0;
const int BASE = 140;
int lastErr = 0;
void drive(int l, int r) {
l = constrain(l, -255, 255); r = constrain(r, -255, 255);
digitalWrite(AIN1, l > 0); digitalWrite(AIN2, l < 0); analogWrite(PWMA, abs(l));
digitalWrite(BIN1, r > 0); digitalWrite(BIN2, r < 0); analogWrite(PWMB, abs(r));
}
void setup() {
int pins[] = {PWMA, AIN1, AIN2, PWMB, BIN1, BIN2, STBY};
for (int i = 0; i < 7; i++) pinMode(pins[i], OUTPUT);
digitalWrite(STBY, HIGH);
pinMode(XS_SIDE, OUTPUT); pinMode(XS_FRONT, OUTPUT);
digitalWrite(XS_SIDE, LOW); digitalWrite(XS_FRONT, LOW); delay(10); // both sensors off
digitalWrite(XS_SIDE, HIGH); delay(10); side.begin(0x30); // wake one and move it to a new address
digitalWrite(XS_FRONT, HIGH); delay(10); front.begin(0x29);
}
void loop() {
VL53L0X_RangingMeasurementData_t ms, mf;
side.rangingTest(&ms, false); front.rangingTest(&mf, false);
int d = (ms.RangeStatus != 4) ? ms.RangeMilliMeter : 300;
int f = (mf.RangeStatus != 4) ? mf.RangeMilliMeter : 800;
if (f < 120) { drive(-120, 120); delay(250); return; } // wall ahead: turn right on the spot
int err = d - TARGET; // positive = too far from the wall
float steer = KP * err + KD * (err - lastErr);
lastErr = err;
drive(BASE - steer, BASE + steer); // too far: slow the left wheel and speed the right to move toward the wall
}Test checklist
What you should see at each check. If a reading is far off, stop and use the troubleshooting list.
| Check | Expected |
|---|---|
| Straight wall | Holds about 150 mm |
| Disturbed | Returns without large swings |
| Corner | Turns right and continues |
| Wall ends | Does not drive in circles forever |
If it does not work
Steers away from the wall
Swap the sign of the steering.
Wobbles
Reduce KP or raise KD.
Sensor not found
Check the XSHUT wiring and the address code.
Motors stutter
Check the battery and the TB6612 supply.
Still stuck? Find the fault step by step with your multimeter →
Common questions
What parts do I need for the wall-following robot with ToF?
Arduino Nano, VL53L0X time-of-flight distance sensor × 2, TB6612FNG Motor Driver, BO motor straight 100 RPM × 2, Robot car chassis kit 2WD (acrylic), Battery holder 4 × AA with switch and AA alkaline battery × 4.
How long does the wall-following robot with ToF take to build?
About 150 minutes for a intermediate maker, in 7 steps.
How do I know the wall-following robot with ToF works?
Straight wall: Holds about 150 mm. Disturbed: Returns without large swings. Corner: Turns right and continues.
Steers away from the wall: what should I check?
Swap the sign of the steering.
Wobbles: what should I check?
Reduce KP or raise KD.
Sensor not found: what should I check?
Check the XSHUT wiring and the address code.
Motors stutter: what should I check?
Check the battery and the TB6612 supply.
