Project tutorial
Take Control Over Lego Power Functions

Take Control Over Lego Power Functions © GPL3+

Learn how to control Lego motors and servos with your Arduino and build your own Android app to remote control your model.

  • 48,311 views
  • 3 comments
  • 68 respects

Components and supplies

About this project

Do you have one of those awesome Lego Power Functions models with electrical motors and servo-motors? In this tutorial I will show you how you can control your Lego model with your Arduino and a minimum of electronic components. I will explain the circuits and programming in detail to make this tutorial easy understandable for beginners. The Lego model you will have to build yourself (if you don't have a son), I won't help you with that.

We will also learn how to use the MIT app inventor to program our own remote control Android app. Soon, you will have your own Lego Mars rover in your backyard!

This is my first Arduino project and my son has a beautiful Lego Technic car model (9398 4X4 Crawler) with Lego Power Functions: one Lego Servo Motor for steering and two Lego L-motors for driving. He allowed me to use the car for this project (as long as I didn't cut up any motor cables). Here, we test the finished prototype:

Backyard testing

Understanding the Power Functions wiring

First, buy some Lego Power Functions Extension wires (item 8886 in the Lego web shop). Next, cut them up (seriously). We will make some "break-out"-cables that have Lego bricks at one end and pins at the other end. Solder the pins on or similar. Here is the wiring explained:

GND stands for Ground which is the negative terminal (-) of the battery pack (anode). C1 and C2 can switch polarity to make the motors and servos switch direction. TechnicRobot has made a nice video explaining everything in detail. (WARNING: you are doing this on your own responsibility. I will not be held responsible if you ruin something!) For the Lego servo connector, you need to solder on pins to all four cables. You can use the light grey plug. For the power cable connecting the battery box with the breadboard, we need only +9 Volt and GND but you have to use the dark grey plug (because you need a male connector):

For the motor cable, we need only to connect C1 and C2 and you can use a light grey plug. (Study the schematics for details).

Controlling a Lego motor with the L293D chip

We want variable speed control for our DC motors and position control for our Lego servo. This is achieved by Pulse Width Modulation (PWM). The Arduino's programming language makes PWM easy to use; simply call analogWrite(pin, dutyCycle), where dutyCycle is a value from 0 to 255. The PWM pins are marked with ~ on your arduino.

The Arduino output pins are 5 Volt and max. 30 mA while the Lego motors need 9 Volt and pull more than 100 mA each. We need some kind of "switching device" in-between. We also want to be able to run the DC motors in both directions. These functions are solved by a so-called H-bridge. We will use the L293D, which contains two H-bridges on one integrated chip, which means we can connect the Lego motors (M) in parallel to one side and the Lego servo (S) to the other side of the chip. (If you want to control the two motors independently you will need a second L293D). The Lego servo also needs to be connected to GND and Lego +9 Volt.

The chip keeps Lego +9 Volt and Arduino +5 Volt completely separated. Never connect them together or you will damage something! But you have to connect all ground lines together and with the L293D ground pins.

From our Arduino, we will use pin 9 to control the motor speed and pins 2 and 5 to control the rotation direction. The Lego servo is controlled like a motor: we connect pin 3 for position and pins 6 and 8 for direction (left or right).

At the top of our program, we define the used Arduino pins as constants. Further, we define some variables that we will use to control the motors:

// Motor control digital output pins defined as global constants
const int controlPin1A = 2;                
const int controlPin2A = 5;                 
const int ENablePin = 9;                   
// Servo control digital output pins defined as global constants 
const int controlPin3A = 6;              
const int controlPin4A = 8;                  
const int servoENablePin = 3;  
        
// Motor control global variables: 
int motorSpeed = 0;                          // Motor speed 0..255
int motorDirection = 1;                      // Forward (1) or reverse (0)
// Servo control global variables:
int steering = 0;                            // Servo position 0..255
int steeringDirection = 0;                   // Left (0) and Right (1)

In the setup, we define these pins as outputs with the pinmode() command and then set them to 0 Volt with digitalWrite()

void setup() 
{
  //other stuff....
 
  // Declare digital output pins:
  pinMode(controlPin1A, OUTPUT);      // 1A
  pinMode(controlPin2A, OUTPUT);      // 2A
  pinMode(ENablePin, OUTPUT);         // EN1,2
  pinMode(controlPin3A, OUTPUT);      // 3A
  pinMode(controlPin4A, OUTPUT);      // 4A
  pinMode(servoENablePin, OUTPUT);    // EN3,4
 
  digitalWrite(ENablePin, LOW);       // motor off
  digitalWrite(servoENablePin, LOW);  // steering centered
}

Now we need to understand how the L293D chip controls the rotation direction of the motors. We have to supply the following signals (High == 5 Volt; Low == 0 Volt):

  • EN1,2     1A       2A
  • High       High   Low     Motor turns left (Forward; motorDirection == 1)
  • High       Low    High    Motor turns right (Reverse; motorDirection == 0)
  • EN3,4     3A     4A
  • High       High   Low     Servo turns left (steeringDirection == 0)
  • High       Low    High    Servo turns right (steeringDirection == 1)

Ok. We are ready to write a subroutine that reads the global variables for direction and speed/position and controls the motors and servo:

void SetMotorControl()
{
 if (motorDirection == 1)               //Forward
   {
      digitalWrite(controlPin1A, HIGH);
      digitalWrite(controlPin2A, LOW);
   }
 else                                   //Reverse
   {
      digitalWrite(controlPin1A, LOW);
      digitalWrite(controlPin2A, HIGH);
   } 
 analogWrite(ENablePin, motorSpeed);    //Speed
 
 if (steeringDirection == 0)            //Left
   {
      digitalWrite(controlPin3A, HIGH);
      digitalWrite(controlPin4A, LOW);
   }
 else                                   //Right
   {
      digitalWrite(controlPin3A, LOW);
      digitalWrite(controlPin4A, HIGH);
   } 
 analogWrite(servoENablePin, steering); //Servo position
}

Simple RC Android app with joystick

Next, let's build a simple Android app to control the model. (If you want to test the finished app first: I have made it available for you in the Google play store. Here is the link: Arduino RC car bluetooth).

We will use the MIT app inventor 2. MIT App Inventor is an innovative beginner's introduction to programming and app creation. Google's Mark Friedman and MIT Professor Hal Abelson co-led the development of App Inventor. It runs as a web service administered by staff at MIT’s Center for Mobile Learning.

On your Android phone, go to Google Play and install the MIT AI2 Companion app. On your computer, open the link to my RC app in either Firefox- or Chrome-browser (Internet Explorer not supported). You will have to log in using your gmail-address and create an account in order to see the source code. If you open the companion app on your phone, you can look at the source code in the browser, make changes to it and test it immediately on your phone.

In the browser, you will now see the Designer-view of the app:

At the bottom, we see the orange pad with the blue joystickball in it. Now change into the Blocks-view to see the program. The heart of the program is a when-block that keeps track of the movements of your finger touching and dragging the blue joystickball:

The position of your finger is tracked in the two variables currentX and currentY and is used to calculate values for the two variables steering and speed (range: -100..0..+100). First the program checks if your finger is outside the pad and limits the values to +/- 100. If the finger is inside the pad, the program calculates values for steering and speed. Next, the program generates a command-string of the form:

"RC,steering,speed,\n"

The command starts with "RC" for remote control (the idea is that in the future, you might want to have more commands) followed by a comma. Then we send the values for steering and speed. The newline-character ('\n') at the end of the command is a signal to the Arduino that the command is complete. This command-string is sent over bluetooth to the Arduino. For your reference, the string is also displayed on the screen.

Reading bluetooth-commands into the Arduino

In order to read the Bluetooth command strings, we need to connect a HC-05 Bluetooth module. Have a look at your module:

Mine has 6 pins. We only need four pins. VCC is (positive) supply voltage and GND is ground. The module tolerates 6 Volt supply voltage, which means we can connect it to the arduino 5 Volt power pin. TXD and RXD are the serial signals. We have to cross the lines and connect TXD to arduino RXD (pin 0) and vice versa. Be careful: It states LEVEL: 3.3V which means that the RXD cannot be directly connected to the 5V arduino TXD (pin 1). We have to construct a voltage divider with three 1 kOhm resistors to produce an output voltage which is 2/3 of 5V (see schematics). On the other hand, the TXD signal of 3.3V can be directly connected to Arduino RXD. The Arduino will recognise 3.3 Volt as HIGH.

As pin 0 and 1 are shared with the USB port of your Arduino, you will have to disconnect  the RXD and TXD signals from pin 0 and 1 while uploading the program over USB to the board. The upload won't work if you don't disconnect the cables.

Next, you have to pair the HC-05 module with your Android device. Switch Bluetooth on; start the app; press "Bluetooth connect"; make your phone visible and look for a device called HC-05 or similar. Select the device. You will be asked for a code. Press "1234". When you return to the joystick-screen, it should state "connected" in green.

Now, lets look at the code:

// Serial buffer size: calculate based on max input size expected for one command
#define INPUT_SIZE 30
 
 
void loop() 
{
  // Get next command from serial bluetooth (add 1 byte for final 0)
  char input[INPUT_SIZE + 1];                  // array of type char (C-string)
  //read Serial until new line or buffer full or time out
  byte size = Serial.readBytesUntil('\n', input, INPUT_SIZE);   
  // Add the final 0 to end the C-string
  input[size] = 0;
 
  // Split string which is of the form:  "RC,steering,speed,\n\0"
  char* command = strtok(input, ",");          // command (RC)   
  // RCsteering in Range: -100 (left).. 0 .. 100 (right)
  char* RCsteering = strtok(NULL, ",");        
  // RCspeed in Range: -100 (full speed reverse).. 0 .. 100 (full speed forward)
  char* RCspeed = strtok(NULL, ",");           
  int iRCsteering = atoi(RCsteering);          // convert string into integer
  int iRCspeed = atoi(RCspeed);                // convert string into integer
 
  //rest of program
}
 

The string sent by the Android app is read into a special string-construct: a C-string with null-termination. (The 0-character tells the program that it has come to the end of the string). This is done by using the function Serial.readBytesUntil('\n', input, ...), which reads from the serial interface (bluetooth) until it gets a newline-character ('\n'). That way, we keep reading bytes until we have a complete command in input:

"RC,steering,speed,\n\0".

A very efficient way to process input is the string token function, which cuts the string into parts using comma as a delimiter. The first call to strtok() returns "RC". The subsequent parts of the command are then read by passing NULL to strtok(). The returning values are stored in RCsteering and RCspeed. These variables are actually pointers to positions in input. The function atoi() converts them finally into integers. Now, we have:

iRCsteering == -100 (left).. 0 .. +100 (right)
iRCspeed == -100 (reverse).. 0 .. +100 (forward)

We are almost done now. We have to multiply these values by 2.55 before we pass them on to our routine SetMotorControl() (Remember that motorspeed was in the range 0..255). Study the rest of the program in the CODE section, build the circuit and test your remote controlled Lego model!

Testing, testing, testing.....

What's next?

Now that you have an Arduino controlling your Lego model, you might want to use that model as a robotics platform: add some ultrasonic ping sensors, program your own obstacle-avoiding logic and make your own self-driving robotic Mars rover. Or add more commands to the remote control and more functions to the model. But first, find a method for the Arduino to figure out that the model has traveled out of range of the bluetooth connection and stop the model. Tell me about your solution.

Soon enough, you will find out that a prototyping breadboard and loose cables are not a good idea for a moving model. Get yourself an adafruit motorshield with big terminal block connectors and rewire and reprogram. This will also free up digital signals that you can use for inputs instead. You might also consider a sensor shield for better connection of your sensors.

Let me know of your exiting projects!

Code

Legocar_v3_bluetoothArduino
/*
 Lego Powerfunctions car model controlled over bluetooth
 Arduino Uno controls motors and servo
 Remote controlled with android app developed with MIT App Inventor 2
 
 Circuit:
 * Serial communication   (uses Uno pin 0,1)    Bluetooth-module is attached (has to be detached when loading program over USB)
 * L293D with motor       (uses Uno digital pins 2,5,9)
 * L293D with servo       (uses Uno digital pins 6,8,3)
 
 
*/

// Used libraries:
#include <SPI.h>                             // Serial Peripheral Interface Library
#include <String.h>                          // contains function strtok: split string into tokens


// Serial buffer size: calculate based on max input size expected for one command over bluetooth serial interface
#define INPUT_SIZE 30

// Motor control digital output pins defined as global constants (4 wheel drive with 2 Lego motors):
const int controlPin1A = 2;                  // L293D driver input 1A on pin no 2  http://www.ti.com/lit/ds/symlink/l293.pdf connected to Arduino digital output pin 2
const int controlPin2A = 5;                  // L293D driver input 2A on pin no 7 connected to Arduino digital output pin 5
const int ENablePin = 9;                     // L293D ENable(1,2) input on pin no 1 connected to Arduino digital output pin 9
// Servo control digital output pins defined as global constants (Servo steering with 1 Lego servo):
const int controlPin3A = 6;                  // L293D driver input 3A on pin no 10 connected to Arduino digital output pin 6
const int controlPin4A = 8;                  // L293D driver input 4A on pin no 15 connected to Arduino digital output pin 8 
const int servoENablePin = 3;                // L293D ENable(3,4) input on pin no 9 connected to Arduino digital output pin 3
// Motor control global variables: 
int motorSpeed = 0;                          // Motor speed 0..255
int motorDirection = 1;                      // Forward (1) or reverse (0)
// Servo control global variables:
int steering = 0;                            // Servo position 0..255
int steeringDirection = 0;                   // Left (0) and Right (1)



void setup() 
{
   Serial.begin(9600);                       // initialize serial communication
   Serial.setTimeout(1000);                  // 1000 ms time out
   // Declare digital output pins:
   pinMode(controlPin1A, OUTPUT);      // 1A
   pinMode(controlPin2A, OUTPUT);      // 2A
   pinMode(ENablePin, OUTPUT);         // EN1,2
   pinMode(controlPin3A, OUTPUT);      // 3A
   pinMode(controlPin4A, OUTPUT);      // 4A
   pinMode(servoENablePin, OUTPUT);    // EN3,4
   digitalWrite(ENablePin, LOW);       // motor off
   digitalWrite(servoENablePin, LOW);  // steering centered
}


void loop() 
{
   // Get next command from serial bluetooth (add 1 byte for final 0)
   char input[INPUT_SIZE + 1];                  // array of type char (C-string) with null-termination (https://www.arduino.cc/en/Reference/String)
   byte size = Serial.readBytesUntil('\n', input, INPUT_SIZE);   //read Serial until new line or buffer full or time out
   // Add the final 0 to end the C-string
   input[size] = 0;

   // Read command which is a C-string of the form:  "RC,RCsteering,RCspeed,\n\0"  (ends with a new line character and a null)
   char* command = strtok(input, ",");          // ignore command for now (RC for Remote Control is the only command sent by the app for the time being)    
   char* RCsteering = strtok(NULL, ",");        // next comes RCsteering which is in the Range: -100 (left).. 0 .. 100 (right)
   char* RCspeed = strtok(NULL, ",");           // next comes RCspeed which is in the Range: -100 (full speed reverse).. 0 .. 100 (full speed forward)
   int iRCsteering = atoi(RCsteering);          // convert string RCsteering into integer iRCsteering
   int iRCspeed = atoi(RCspeed);                // convert string RCspeed into integer iRCspeed
   
   // Set motor and servo control variables
   if (iRCsteering > 0) steeringDirection = 1;   // turn right if iRCsteering = 1..100
   else steeringDirection = 0;                   // turn left  if iRCsteering = -100..0
   steering = int(2.55*abs(iRCsteering));        // Servo position 0..255 based on command 0..100
   if (iRCspeed > 0) motorDirection = 1;         // Set car in forward if iRCspeed = 1..100
   else motorDirection = 0;                      // Set car in reverse if iRCspeed = -100..0
   motorSpeed = int(2.55*abs(iRCspeed));         // Set speed 0..255 based on command 0..100
   
   SetMotorControl();                            // adjust motor direction and speed 
}




void SetMotorControl()
/*
L293 logic:    EN1,2   1A    2A
               H       H     L    Motor turns left  (Forward; motorDirection == 1)
               H       L     H    Motor turns right (Reverse; motorDirection == 0)
               
               EN3,4   3A    4A
               H       H     L    Servo turns left  (steeringDirection == 0)
               H       L     H    Servo turns right (steeringDirection == 1)

Motor speed:   PWM signal on EN1,2 (490 Hz; digital output value 0..255 for motorSpeed)
Servo position:PWM signal on EN3,4 (490 Hz; digital output value 0..255 for position; 0 is straight ahead)
*/
{
  if (motorDirection == 1)               //Forward
    {
       digitalWrite(controlPin1A, HIGH);
       digitalWrite(controlPin2A, LOW);
    }
  else                                   //Reverse
    {
       digitalWrite(controlPin1A, LOW);
       digitalWrite(controlPin2A, HIGH);
    } 
  analogWrite(ENablePin, motorSpeed);    //Speed
    
  if (steeringDirection == 0)            //Left
    {
       digitalWrite(controlPin3A, HIGH);
       digitalWrite(controlPin4A, LOW);
    }
  else                                   //Right
    {
       digitalWrite(controlPin3A, LOW);
       digitalWrite(controlPin4A, HIGH);
    } 
  analogWrite(servoENablePin, steering); //Servo position

}
Github Lego-car-Arduino
Contains source code Arduino; Fritzing diagram; source code android app for uploading to MIT app inventor

Schematics

Github Lego-car-Arduino
Contains source code Arduino; Fritzing diagram; source code android app for upload to MIT app inventor
Fritzing diagram breadboard Lego-car-Arduino
Fritzing diagram Lego car
Lego%20rc%20car%20v2.fzz
Fritzing diagram schematics Lego-car-Arduino

Comments

Similar projects you might like

IoT Dune Buggy - Control it from Anywhere!

Project tutorial by Satyavrat Wagle

  • 9,425 views
  • 7 comments
  • 29 respects

Arduino and LEGO® Power Functions

Project tutorial by Tiago Santos

  • 12,110 views
  • 0 comments
  • 23 respects

Simple Arduino Based Lego Power Function Receiver

Project tutorial by Arduino_Scuola

  • 3,810 views
  • 1 comment
  • 5 respects

3D Printed R2-D2 with an Arduino Core

Project showcase by Raul Antonio

  • 3,245 views
  • 1 comment
  • 4 respects
Add projectSign up / Login