Project tutorial
Autobot Using Lego NXT Motors and Sensor

Autobot Using Lego NXT Motors and Sensor

Project that uses lego NXT motors and a sensor to avoid objects.

  • 30 respects

Components and supplies

Ph a000066 iso (1) ztbmubhmho
Arduino UNO
HC-SR04 Sensor
Adafruit Motor Breakout
LEGO MindStorms Lego NXT Motor
Adafruit Micro Servo
Breadboard Power Suppy
Robot Chassis or build your own
9V battery (generic)
Keystone 233 image 75px
9V Battery Clip

Necessary tools and machines

Wire cutters
09507 01
Soldering iron (generic)

Apps and online services

Ide web
Arduino IDE
Used to upload the code to Uno

About this project


Like most of my projects, I like to re-use parts I have laying around.  This project is no different.  I found some Lego NXT motors that I decided to control via Arduino and what better way to use motors than to build a robot that wanders around and avoids obstacles?

This project isn't super difficult, just a little time consuming.


  • 2  X  Lego NXT motors
  • 2  X  NXT connector wires
  • 1  X  motor breakout
  • 1  X  HC-SR04 sensor
  • 1  X  Arduino (I used a UNO)
  • 1  X  Breakout proto board
  • 1  X  Pan/Tilt kit with servo (You will only use the Pan function)
  • Jumper wires
  • Power breakout board (Used for the motor breakout, you could plug a 9V battery directly into the motor breakout)
  • Power for the Uno and the motor breakout.  I am using two 9V batteries.
  • Some sort of chassis to mount everything on.  I am just using foam board.

Step 1 - Motors

The first task that needed to be done was to see how the Lego NXT motor worked.  The nice thing is, Lego actually provides schematics of the NXT products!  I downloaded the ZIp file and looked at the schematic for the controller and discovered the following for the motor connector:

PIN       Color       Name

1           White       MA0

2           Black       MA1

3           Red         GND

4           Green      4.3V (from NXT Control Unit)

5           Yellow     Tach01  (rotary Encoder)

6           Blue        Tach02 (rotary encoder)

Note:  We will only be using Pin 1 and Pin 2 for this project.

This 9V Lego motor is pretty sophisticated as it has a rotational rotary encoder with 1 degree of resolution and is connected to the Yellow and Blue wires.  I will keep this in mind for a future project!    Also, for this project I will only be using pins 1 and 2 (white and black wires) connected to Adafruit motor breakout board using 5V.

To connect the motor to the motor breakout you will need the Lego wire connectors.  They have a special RJ12 connector with the tab being offset.  Since I didn't have any of the connectors and I am too lazy to order any I just cut the end off of one side, stripped the wires back and tinned the stranded wire.  You can also buy NXT motor breakout boards and connectors.  Check out

Step 2 - HC-SR04

Sonar is used to determine the distance of an obstacle.  According to the datasheet it has a range of 2cm to 400cm and has a effectual angle of 15 degrees.

To get a distance, the Trig of the sensor gets a pulse of at least 10uS which transmits a 8 cycle 40kHz ultrasonic burst then waits for the response.  When the sensor detects the ultrasonic burst on the receiver, the SR04 Echo pin will go high and delay for a time which is proportional to the distance.  So to get the distance we measure the width of the Echo pin. And in the code you will see that we get a distance in centimeters by taking the width * .035 / 2.

The method to do all of this is pretty simple as you can see in the readSensor function of the code.

Connect the sensor as follows:

  • VCC  -->   Breadboard 5V rail (Red)
  • Trig    -->  Pin 4 (Yellow) on Arduino
  • Echo  -->  Pin 7 (Green) on Arduino
  • GND  -->   Breadboard GND rail (Black)

Mount sensor to the pan/tilt mechanism and mount the pan/tilt to you chassis.  I mounted the sensor upside down on the pan/tilt kit so it was easier to connect the wires.

Step 3 - Motor Breakout

I am utilizing the Adafruit DC/Stepper Motor driver.  it utilizes the TB6612 driver IC.  This IC can control two DC motors or 1 stepper motor and supply up to 1.2A.

Connect the sensor as follows:

  • VCC  -->   Breadboard 5V rail
  • GND  -->   Breadboard GND rail
  • PWMA  -->  Pin 3 on Arduino
  • AIN1  -->  Pin 8 on Arduino
  • AIN2  -->  Pin 9 on Arduino
  • PWMB  -->  Pin 5 on Arduino
  • BIN1  -->  Pin 11 on Arduino
  • BIN2  -->  Pin 12 on Arduino
  • Motor A  -->  NXT Lego motor A (white and black wire)
  • Motor B  --> NXT Lego motor B (white and black wire)
  • Vmotor  -->  Connected to power breakout board, 5V pins

Step 4 - Servo

I am using a inexpensive micro servo that came with the pan/tilt kit.

Connect the pan servo as follows:

  • VCC (Red)  --> Breadboard 5V rail
  • GND (Brown)  --> Breadboard GND rail
  • Control Orange)  -->  Pin 6
Note: Servo is shown removed from the pan/tilt mechanism for clarity.

Step 5 - Code

Upload the code to the Arduino.  The code sets up the sensors and motors then starts using the sensor to detect obstacles.  If an obstacle is detected the sensor scans right and left to determine which direction has more room and turns the robot in that direction.

There are a few variables that you can modify to help make the robot detect and avoid obstacles better. 

checkDist - This is the distance used before the robot scans left and right.  If the distance is less than or equal to this variable, start scanning.

rightAngle - This is the right position the servo will set once a obstacle is detected.

leftAngle - This is the left position the servo will set once a obstacle is detected.

centerAngle - This is center point the servo will set after scanning left and right.

mtrSpeed - Regular forward speed

turnSpeed used to turn the robot by making one motor turn faster than the other.  Other turning options are to just stop one of the motors.  I used this method to keep the robot moving forward but feel free to experiment.

Note: Serial.print commands are used to troubleshoot the sensor but printing out the distance to the serial monitor.

Step 6 - All Connected!

I used 2 9V batteries to power the UNO and the motors.  

  • 9V battery 1  -->  Uno
  • 9V battery 2  -->  5V power breakout
Note:  Only one servo is connected (Pan servo)

Misc. Pictures

Here are a couple more pictures.

See it in Action!


Final Thoughts

Here are some ideas for modifications:

  • Make use of the motor encoders
  • Use both the pan and tilt
  • Bluetooth or WiFi enable the robot

As always, feel free to comment and hope this is entertaining and  useful.  Thanks for looking!


Upload to UNO using IDE
 * Autobot
 * Code moves a robot and uses a SR04 sensor to detect objects
 * All Serial.print statements are used to troubleshoot and can be removed if desired

#include <Servo.h>      //Used for the sensor servo

// Setup Echo Sensor
int TRIG_PIN = 4;       //Trigger pin
int ECHO_PIN = 7;       //Echo pin 
int checkDist = 70;     //Distance allowed before turning
int senReading = 0;

//Servo setup
Servo sensorServo;      // create servo object to control a servo
int servoPin = 6;       //Servo on Pin defined
int leftAngle = 130;    //Servo look left angle
int rightAngle = 45;    //Servo look right angle
int centerAngle = 90;   //Center angle

//Motor A
int PWMA = 3;   //Speed control pin 
int AIN1 = 8;   //Direction - HIGH or LOW
int AIN2 = 9;   //Direction - HIGH or LOW

//Motor B
int PWMB = 5;   //Speed control pin 5
int BIN1 = 11;  //Direction - HIGH or LOW
int BIN2 = 12;  //Direction - HIGH or LOW

/* Motor speed control
 *    mtrSpeed - The normal forward speed
 *    turnSpeed - used to turn the robot by making one motor turn faster than the other
int mtrSpeed = 100;     //regular robot speed
int turnSpeed = 200;    //faster speed used to turn robot

void setup(){
      Serial.begin(9600);           //Setup Serial monitor for 9600 baud

      pinMode(PWMA, OUTPUT);        //Motor A
      pinMode(AIN1, OUTPUT);
      pinMode(AIN2, OUTPUT);

      pinMode(PWMB, OUTPUT);        //Motor B
      pinMode(BIN1, OUTPUT);
      pinMode(BIN2, OUTPUT);

      pinMode(TRIG_PIN, OUTPUT);    //Sensor Trigger
      pinMode(ECHO_PIN, INPUT);     //Sensor Echo 

      sensorServo.attach(servoPin); //Attach servo on pin 9 to the servo object
      sensorServo.write(90);        //Center servo  

//Start moving
      move(1, mtrSpeed, 1);
      move(2, mtrSpeed, 0);  

      delay(1000);            //Delay 1 second to let robot get settled

 * Main Program Loop
void loop() {
senReading = readSensor();          //Read the sensor
 if (senReading <= checkDist) {     //Sensor detected obstacle
      Serial.println ("Obstacle detected, scan for clear path...");
      Serial.print("Center: ");      Serial.print(senReading);    //Print center sensor reading to serial monitor

//Turn sensor to right and scan
      int rVal = readSensor();
      Serial.print("\tRight: ");    Serial.print(rVal);    //Print right sensor reading to serial monitor

//Turn sensor to left and scan
      int lVal = readSensor();
      Serial.print("\tLeft: ");    Serial.println (lVal);    //Print left sensor reading to serial monitor

//Re-center sensor
      if (rVal > lVal) {      //determine which direction to turn
            move(2, turnSpeed,0);
            delay (2000);
            } else {
            move(1,turnSpeed, 1);      
            delay (2000);      
    } else {    //No obstacle, moving along
    move(1, mtrSpeed, 1); 
    move(2, mtrSpeed, 0);  

 * Move motor logic
void move(int motor, int speed, int direction) {

/*Move specific motor at speed and direction
*     motor: 1 for Motor A, 2 for motor B
*     speed: 0 is off, and 255 is full speed
*     direction: 0 clockwise, 1 counter-clockwise

      boolean inPin1 = LOW;
      boolean inPin2 = HIGH;

      if(direction == 1){
            inPin1 = HIGH;
            inPin2 = LOW;

      if(motor == 1) {
            digitalWrite(AIN1, inPin1);         //Used to detremine motor direction
            digitalWrite(AIN2, inPin2);         //Used to detremine motor direction
            analogWrite(PWMA, speed);     
      } else {
            digitalWrite(BIN1, inPin1);         //Used to detremine motor direction
            digitalWrite(BIN2, inPin2);         //Used to detremine motor direction
            analogWrite(PWMB, speed);

 *  Read sonar sensor and return distance in cm

float readSensor () {
      int distance = 0;
      digitalWrite(TRIG_PIN, LOW);        // Set pin Low
      digitalWrite(TRIG_PIN, HIGH);       //Set trigger pin high for 10uS
      digitalWrite(TRIG_PIN, LOW);        //Set pin low and determine distance
      distance = pulseIn(ECHO_PIN, HIGH) * 0.035 /2;        //Read "width" and convert to CM
      return distance;


Parts Layout
Parts Layout


Similar projects you might like

Make your first Arduino robot - The best beginners guide!

Project tutorial by Muhammed Azhar

  • 155 respects

Exploring Encoders - Step Trackers for Motors

Project tutorial by SciJoy

  • 9 respects

Lego Stepperbot

Project tutorial by Fredrik Stridsman

  • 10 respects

Soil Moisture Sensor With LCD Display

Project tutorial by Patel Darshil

  • 57 respects

LEGO Wall-E with Arduino

Project tutorial by monsterbacke

  • 100 respects

Take Control Over Lego Power Functions

Project tutorial by Patrick Müller

  • 76 respects
Add projectSign up / Login