Project tutorial

Autobot Using Lego NXT Motors and Sensor

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

  • 12,482 views
  • 5 comments
  • 22 respects

Components and supplies

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

Intro

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.

Parts:

  • 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 mindsensors.com.

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!

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!

Code

autobotC/C++
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
      sensorServo.write(rightAngle);
       delay(300);      
      int rVal = readSensor();
      Serial.print("\tRight: ");    Serial.print(rVal);    //Print right sensor reading to serial monitor


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

      
//Re-center sensor
      sensorServo.write(centerAngle);
      delay(300); 
    
      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
      delayMicroseconds(2); 
      digitalWrite(TRIG_PIN, HIGH);       //Set trigger pin high for 10uS
      delayMicroseconds(10); 
      digitalWrite(TRIG_PIN, LOW);        //Set pin low and determine distance
      distance = pulseIn(ECHO_PIN, HIGH) * 0.035 /2;        //Read "width" and convert to CM
  
      delay(50);
    
      return distance;
}

Schematics

Schematic
Parts Layout
Schematic
Schematic
Parts Layout
Schematic

Comments

Similar projects you might like

Rotating Platform!

Project showcase by mPelectronic

  • 482 views
  • 2 comments
  • 6 respects

High Precision Scale with Arduino

Project showcase by mPelectronic

  • 4,729 views
  • 11 comments
  • 23 respects

Kravox! Wireless, Motion and Touch-Sensing Instrument

Project tutorial by Tim Krahmer

  • 3,811 views
  • 2 comments
  • 20 respects

Automatic Flying Bug Killer

Project tutorial by Mitomon

  • 575 views
  • 3 comments
  • 4 respects

Arduino HID-Based CNC Pendant

Project tutorial by edr1924

  • 1,877 views
  • 4 comments
  • 7 respects

GPS Tracking System for Vehicles

Project showcase by Sustainable Living Lab

  • 5,883 views
  • 2 comments
  • 29 respects
Add projectSign up / Login