Mark Tashiro
Published

Autobot Using Lego NXT Motors and Sensor

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

IntermediateFull instructions provided2 hours22,024
Autobot Using Lego NXT Motors and Sensor

Things used in this project

Hardware components

Arduino UNO & Genuino UNO
Arduino UNO & Genuino UNO
×1
HC-SR04 Sensor
×1
Adafruit Motor Breakout
×1
LEGO MindStorms Lego NXT Motor
×2
Adafruit Micro Servo
×1
Breadboard Power Suppy
×1
Robot Chassis or build your own
×1
9V battery (generic)
9V battery (generic)
×2
9V Battery Clip
9V Battery Clip
×2

Software apps and online services

Arduino IDE
Arduino IDE
Used to upload the code to Uno

Hand tools and fabrication machines

Wire cutters
Soldering iron (generic)
Soldering iron (generic)

Story

Read more

Schematics

Schematic

Parts Layout

Schematic

Parts Layout

Code

autobot

C/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;
}

Credits

Mark Tashiro

Mark Tashiro

5 projects • 54 followers
Love gadgets. Passion for figuring out how things work.
Thanks to Adafruit.

Comments