How to Build Your Own Version of Lego Mindstorms With Arduino for ~ $100

Hello reader!

In this tutorial I will show you how I made my own version of Lego Mindstorms.

You will wonder: why?

Lego Mindstorms is a quite good set to get into robotics while being able to use legos to construct the robot. But this set is very expensive (more than 400$). So I decided to build my own version of it with way less money and using the Lego Power Functions Set I had lying around with many other components I used. Since I got bored of building Lego cars that can only move it's the perfect project to take it to the next level. The idea is writing a file in an SD card that arduino can load, read and act accordingly to what it reads. It's an Interpreter. An Interpreter is a program that reads a file and does something when it finds a text defined as a command inside the code. For example if it is programmed to print hello when it reads hello inside a file, it will print hello on the screen whenever it finds "hello" inside the file it opened. This will be done for controlling the DC and Servo motors and do other things too.

So what about the supplies?

You need a lot of stuff to make it.

Arduino Mega 2560

Catalex Micro SD card Adapter (Doesn't really matter if it's not Catalex BUT you may need another library or extra circuetry)

SD memory card (Even 2GB of capacity are enough)

I2C 16x2 LCD

L293D motor driver

3xPush-Buttons

5V LED of your choice (It will be used for testing if the system works)

2xMicro Servo SG90

2xPower Functions Medium motor

2xPower Functions Extension Wire (Both the short and the long one are ideal)

Power Functions Battery Box

Power Bank (In this point, both the battery box and a power bank are needed in order to power everything)

Breadboard

3x10ΚΩ resistor

10Ω resistor (It's for the LED so get it only in case you want to add the LED)

USB type-B to type-A cable

A lot of jumper wires (male-to-male, male-to-female, female-to-female)

Soldering Wire

Some Lego parts :)

Tools needed:

Soldering Iron

Wire Cutter

Let's begin by assembling the circuit. It will become really messy in the end so I suggest tying wires of each module like in the second picture . But first, take the two PF (short for Power Functions) Extension cables and cut them in the half. Then solder a male-to-male wire to each of the four wires of the plug. I recommend using red for 9V, black for GND, C1 and C2 can be yellow and green, both yellow or whatever you want because these two wires are these two that can change polarity to rotate the motors in both directions while 9V and GND are for power. Then secure all the wires with insulating (a.k.a. duct) tape. Make sure that you use a dark grey plug for power from the battery box and not a light grey. After assembling the circuit as shown in pictures 3-7, what's next is the program!

The first thing needed to begin is including the libraries. (See the photo).

Now let's define some things:

// An object that will be used to open files
File myFile;
Servo ServoC; // An object that controls a servomotor
Servo ServoD;

#define okButton 11 // Define the pin that the "OK" button will be connected(11)
#define upButton 32
#define downButton 33
#define Motor1Pin1 2 // Pin definitions for the motor driver.

//Motor Pins are for the direction and ENable Pins are for the speed
#define Motor1Pin2 5
#define ENablePin1 9
#define Motor2Pin1 6
#define Motor2Pin2 8
#define ENablePin2 3

#define LedPin 22

// Maximum number of files arduino is allowed to load in it's dynamic memory (RAM)
#define MAX_PROGRAMS 3

//Create all the necessary variables:

char* result[2];
int lcdColumns = 16;
int lcdRows = 2;
int motor1Speed = 0; // Motor speed 0..255
int motor1Direction = 1;
int motor2Speed = 0; // Motor speed 0..255
int motor2Direction = 1;

int ServoCdir = 0;
int ServoDdir = 0;

int FileNumber = 0;
//PAY ATTENTION HERE:
// programs can only be a number and then .PRG
// otherwise a way more complicated code would be required

//these two are for the LCD to turn off the backlight after a certain amount of time to save energy
int statusLCD;
unsigned long sleepLCD;

bool OkButvalue = 0;
bool UpButvalue = 0;
bool DownButvalue = 0;
bool firstTime = true;
int pinCS = 53;
char fileName[7] = "1.PRG";
char* programs[MAX_PROGRAMS];
String readString;
bool runningProg = false;

LiquidCrystal_I2C lcd(0x27, 16, 2);
// You may need to change 0x27 to another HEX address

#define FILE_FORMAT_VERSION 1
// I will explain later its usage

This step is what will be done in the function called setup that runs when arduino has started operating

void setup() {

pinMode(okButton, INPUT);
pinMode(upButton, INPUT);
pinMode(downButton, INPUT);
pinMode(Motor1Pin1, OUTPUT); // 1A
pinMode(Motor1Pin2, OUTPUT); // 2A
pinMode(ENablePin1, OUTPUT); // EN1
pinMode(Motor2Pin1, OUTPUT); // 3A
pinMode(Motor2Pin1, OUTPUT); // 4A
pinMode(ENablePin2, OUTPUT); // EN2

//initialize motor speed to 0
digitalWrite(ENablePin1, LOW);
digitalWrite(ENablePin2, LOW);

pinMode(LedPin, OUTPUT);
pinMode(pinCS, OUTPUT);

//serial port baud rate
Serial.begin(9600);

//the pin the servo will be connected to
ServoC.attach(4);
ServoD.attach(10);

//necessary setup for the LCD
lcd.init();
lcd.backlight();

//initialize lcd's cursor
lcd.setCursor(0, 0);

Serial.println(F("Starting..."));

//clear the display
lcd.clear();
lcd.setCursor(0, 0);
lcd.print(F("Starting..."));

delay(1000);

Serial.println(F("Beginning SD card..."));

if (!SD.begin()) {

Serial.print(F("Error reading SD card"));

lcd.clear();
lcd.setCursor(0, 0);
lcd.print(F("Error reading SD card"));

while (1);
}

Serial.println(F("SD begin done."));

//A variable for servo's direction
ServoCdir = 0;

//initialize servo's direction
ServoC.write(ServoCdir);

ServoDdir = 0;
ServoD.write(ServoDdir);

}

void readCardFiles() {

// if it's not true then no program is running and arduino doesn't need to do anything
if(runningProg == true) {

//this is to check if the OK button has been pressed so far
if(firstTime == false) {

File rootDirectory = SD.open("/");

Serial.println(F("readCardFiles"));

getFileList(rootDirectory, fileName);

}

}

}

This function scans all the SD card for files with the extension .PRG (as shown in the screenshot).
There is no special formating in this file, it is just a simple text.
It's just a way for arduino to know that it's a program for it to read.
Each program must have a header so that arduino has a second way to recognize programs.
Each program also has a number which is the version.
It must be the same defined on the arduino code and the .PRG file.
If the program is written for a higher version, it will not run it.
It a kind of compatibility test between code and data in file.

void getFileList(File dir, String fileToRead) {

Serial.println(F("getFileList"));

String line;

//boolean that indicates whether the version of the program is equal or less to arduino's code version
bool version_ok;
bool header_ok;

//indicates if the interpreter has started reading the program
bool startReadingCommands;

int fileIdx = 0;

//Run forever
while (true) {

//open the first file
File entry = dir.openNextFile();

//if there are no more files
if (! entry) {

Serial.println(F("No more files"));

//There are no more files, exit the loop
break;

}

Serial.print(F("Found file:"));
Serial.println(entry.name());

String test = entry.name();

//Is it the file we are searching or not?
if (!test.equalsIgnoreCase(fileToRead)) {

Serial.println(F("Not this"));
entry.close();
continue;

}

//if it can't load more files
if (fileIdx == MAX_PROGRAMS) {

Serial.println(F("Cannot add more files. Array is full"));
entry.close();
continue;

}

Serial.print(F("Try to read file:"));
Serial.println(entry.name());

myFile = SD.open(entry.name(), FILE_READ);
header_ok = false;
version_ok = false;
startReadingCommands = false;
programs[fileIdx] = NULL;

while (myFile.available()) {

//Read the entire line
line = myFile.readStringUntil('\n');

//and remove leading and tailling spaces
line.trim();

//Ignore lines that starting with #
//This character is used to let us write comments into the file
if (line.startsWith("#")) {

Serial.println(F("Inline comment:"));
Serial.print(line);

} else {

// if the reader has reached the end of the program
if (header_ok && version_ok && line.equals("PROGRAM_END")) {

Serial.println(F("\nstop reading commands"));
startReadingCommands = false;
Serial.println(F("Total program:"));
Serial.println(programs[fileIdx]);

//this is the whole program.
//It can be splitted in different lines but the code can still get it

fileIdx++;

}

//if everything OK with the program and
if (header_ok && version_ok && startReadingCommands) {

int len = line.length();

if (len > 0) {

Serial.println(F("adding program:"));
Serial.println(line);

if (programs[fileIdx] == NULL) {

//allocate the needed amount of bytes in the RAM
programs[fileIdx] = malloc(len + 1);
strcpy(programs[fileIdx],
line.c_str());

} else {

int old_size = strlen(programs[fileIdx]);
programs[fileIdx] = realloc(programs[fileIdx], old_size + len + 1);
strcpy(programs[fileIdx] + old_size, line.c_str());

}
} else {

Serial.println(F("Ignoring empty line"));

}
}

//if true, then it's a program
if (line.equals("PDRS_ROBOT")) {

header_ok = true;
Serial.println(F("Header is ok"));

}

if (header_ok && line.startsWith("VERSION:")) {

Serial.println(F("Reading version"));
line.replace("VERSION:", "");

//if true the versions are compatible
if (line.toInt() <= FILE_FORMAT_VERSION) {

Serial.println(F("version is ok"));
version_ok = true;

} else {

Serial.println(F("found:"));
Serial.print(line);

}
}

//if true then below "PROGRAM_START" it's the actual program

if (header_ok && version_ok && line.equals("PROGRAM_START")) {

Serial.println(F("start Reading Commands"));
startReadingCommands = true;

}
}
}

myFile.close();
entry.close();

}

Serial.println(F("Operation completed"));

}

Before going to program execution some more functions are needed:

splitText splits the given text in the given delimiter, removes the delimiter and returns two texts.
result[0] is the text before the delimiter and result[1] is the text after the delimiter.

int splitText(char* text, char* delimiter, char *myArray[]) {

char * token = strtok(text, delimiter);
int i = 0;

while (token != NULL ) {

myArray[i++] = token;
token = strtok(NULL, delimiter);
}

return i - 1;
}

applyMotorPar applies the motor parameters as it says.
In other words, it sets the direction and the speed of each DC motor to the corresponding variables

void applyMotorPar() {

if (motor1Direction == 0) {

// Pin1, LOW and Pin2 HIGH means backward while the opposite means forward
digitalWrite(Motor1Pin1, HIGH);
digitalWrite(Motor1Pin2, LOW);

} else {

digitalWrite(Motor1Pin1, LOW);
digitalWrite(Motor1Pin2, HIGH);

}

//Send an analog (0-255) signal to the motor driver to change the motor speed
analogWrite(ENablePin1, motor1Speed);

//print direction and speed of motor 1
Serial.println(F("Motor 1:"));
Serial.print(motor1Direction);
Serial.print(F(" Speed:"));
Serial.print(motor1Speed);
Serial.print("\n");

if (motor2Direction == 0) {

digitalWrite(Motor2Pin1, HIGH);
digitalWrite(Motor2Pin2, LOW);

} else {

digitalWrite(Motor2Pin1, LOW);
digitalWrite(Motor2Pin2, HIGH);

}

analogWrite(ENablePin2, motor2Speed);

//print direction and speed of motor 2
Serial.println(F("Motor 2:"));
Serial.print(motor2Direction);
Serial.print(F(" Speed:"));
Serial.print(motor2Speed);
Serial.print("\n");

}

//A function to print in the LCD is needed too:
void Print (String line1, String line2) {

//Set the last state of the LCD backlight
statusLCD = HIGH;

lcd.setBacklight(HIGH);
lcd.clear();

//first row first column
lcd.setCursor(0, 0);

lcd.print(line1);

//second row first column
lcd.setCursor(0, 1);

lcd.print(line2);

//save the timestamp for later usage
sleepLCD = millis();

}

Last thing before program execution is syntax.
Syntax is the rules that define the combinations of symbols that are considered to be correctly structured commands and their parameters in that language.
Please read this step till the end so that you understand what's going on in the next step

The first part is the command.
Then a colon character ":" is used to seperate the command from its parameters.
Afterwards, the parameters are written without any spaces.
Finally, a semicolon character ";" is used to say the interpreter that the command ends

Examples:
A:f255;
This command sets the motor A to direction f (forward) with speed 255 (maximum)

B:b155;
This command sets the motor B to direction b (backward) with speed 155 (it's close to 50% but the motor can barely rotate under 150)

C:90;
This command tells the motor C to rotate at 90 degrees position. Minimum is 0 and maximum 180

W:1000
Tells the interpreter to wait 1000 milliseconds (1 second)

X;
Stops the program execution

L:1;
Turns the LED on.
Use the command "L:0;" to turn it off

Before executing the command, it has to be found and seperated from other commands.
That's what findCmd does:

//"text" parameter is the actual program that will be executed by the interpreter

void findCmd(char* text) {

//check if the program is running
if(runningProg){

Serial.println(F("finding command"));
String lala = text;
Serial.println(lala);
String command;

//it finds the first semicolon in the program
int idx = lala.indexOf(';');

Serial.println(idx);

//while there is actually a semicolon inside the program
while (idx != -1) {

//the command with it's parameters joined
command = lala.substring(0, idx);
Serial.print(F("Sending for check: "));
Serial.println(command);

//remove the current command from the queue
lala.remove(0, idx + 1);

//call the function that executes the commands and pass as parameter the current command we found
execute(command);

idx = lala.indexOf(';');

}
}
}

Finally, the function that executes the commands:

void execute(String command) {

//if the program is running
if(runningProg == true) {

char *cmd;
char *param;
boolean hasParams = false;

Serial.println(F("Checking command:"));
Serial.println(command);

//seperate the command from the parameters, if any
int x = splitText(command.c_str(), ":", result);

cmd = result[0];

//if the command has any parameters
if (x > 0) {

param = result[1];

}

Serial.println(F("command:"));
Serial.println(cmd);
Serial.println(F("param:"));
Serial.println(param);

//change servo's direction
if (cmd[0] == 'C') {

ServoC.write(atoi(param));
Serial.println(F("ServoC:"));
Serial.print(param);

}
//change servo's direction
else if (cmd[0] == 'D') {

//parameters are a text (even numbers are considered to be text in a string)
//atoi function converts them to numbers

ServoD.write(atoi(param));
Serial.println(F("ServoD:"));
Serial.print(param);

}
//wait command
else if (cmd[0] == 'W') {

Serial.println(F("Waiting "));
Serial.print(param);
Serial.print(F("ms"));
delay(param);

}
//Stop program execution
else if (cmd[0] == 'X') {

myFile.close();
runningProg = false;
motor1Speed = 0;
motor2Speed = 0;
applyMotorPar();
Serial.println(F("ProgramStop"));

}
//print on LCD command
else if (cmd[0] == 'P') {

lcd.clear();
lcd.setCursor(0, 0);
lcd.print(param);
Serial.println(F("Printing:"));
Serial.print(param);

}
// command to change parameters on motor A
else if (cmd[0] == 'A') {

//if the first character is "f"
if (param[0] == 'f') {

motor1Direction = 1;
motor1Speed = atoi(param + 1);

} else if (param[0] == 'b') {

motor1Direction = 0;
motor1Speed = atoi(param + 1);

}

//after changing the speed and direction values of the motor, apply them
applyMotorPar();

}

// command to change parameters on motor B
else if (cmd[0] == 'B') {

if (param[0] == 'f') {

motor2Direction = 1;
motor2Speed = atoi(param + 1);

} else if (param[0] == 'b') {

motor2Direction = 0;
motor2Speed = atoi (param + 1);

}

applyMotorPar();

}

}

// command to change the LED statement
else if (strcmp(cmd, "L") == 0) {

Serial.print(F("Changing Led statement"));
Serial.print(param[0]);

if (strcmp(param, "0") == 0) {

digitalWrite(LedPin, LOW);
Serial.println(F("\nSetting Led to LOW"));

} else if (strcmp(param, "1") == 0) {

digitalWrite(LedPin, HIGH);
Serial.println(F("\nSetting Led to HIGH"));

}

} else {

lcd.clear();
lcd.setCursor(0, 0);
lcd.print(Invalid cmd);
Serial.println("E2");

}

} else {

//if the variable "runningProg" is false then this function should not be running
return;

}
}

After creating the loop function and uploading the entire program to arduino, the robot will come to life!

void loop() {

delay(200);

// test if the button is pressed or not
OkButvalue = digitalRead(okButton);
UpButvalue = digitalRead(upButton);
DownButvalue = digitalRead(downButton);

Serial.println(fileName);

//if 4 seconds have passed since the last screen refresh

if ( (statusLCD == HIGH) && (millis() - sleepLCD >= 4000) ) {

// Backlight off
statusLCD = LOW;
lcd.setBacklight(LOW);

}

//if no program is running
if (!runningProg) {

//if "down button" is pressed
if (DownButvalue == 1) {

delay(200);
Serial.println(F("DB pressed"));

if (FileNumber > 1) {

FileNumber--;

// combine the number and the file format
sprintf(fileName, "%d.PRG", FileNumber);
Serial.println(fileName);
Print(F("Select program"), fileName);

}

}

// if "up button" is pressed
if (UpButvalue == 1) {

delay(200);
Serial.println(F("UB pressed"));

if (FileNumber < MAX_PROGRAMS) {

FileNumber++;
sprintf(fileName, "%d.PRG", FileNumber);

// if a file with this name actually exists
if (SD.exists(fileName)) {

sprintf(fileName, "%d.PRG", FileNumber);
Serial.println(fileName);
Print(F("Select program"), fileName);

} else {

FileNumber--;
sprintf(fileName, "%d.PRG", FileNumber);

}
}
}

//if "OK/enter" button is pressed
if (!runningProg && OkButvalue == 1) {

Print(F("Running"), fileName);

//set that the button has been pressed at least one time
firstTime = false;

//set that a program is running
runningProg = true;

//the function to load the program
readCardFiles();

//locate the first command
findCmd(programs[0]);

Serial.println(F("Running Program"));
Serial.println(fileName);

}

if (runningProg) {

Serial.println("spliting a command");

//Finally start the interpreter.
//Parameter "programs[0]" contains the entire script that will be executed
findCmd(programs[0]);

}

}

I am planning to build an enclosure, add sensors like ultrasonic, more commands like "IF", "LOOP".
I have already started the version 2 of this robot that will be controlled via bluetooth module and an Android application.

This is just the beginning...

The Lego_Hybrid_robot.ino file is the program (source code) to compile and upload to the Arduino Mega.

In this code I have some extra functions (such as the bluetooth communication) that are not finished and are not supported by the current hardware, but will be supported in Lego Hybrid V2 that is under development.

Files 1.txt and 2.txt are example programs you can copy-paste to your SD card and run.

BUT FIRST rename them to 1.PRG and 2.PRG.
(instructables cannot upload files with PRG extension)

And some photos of Lego Hybrid on a Lego chassis!