Arduino Lego Robot – Update 1

I’ve been building  an Arduino powered Lego tracked robot on and off for the past half year, and have finally come to the point where there’s something to show.

I didn’t take long to realize that I had to become proficient in programming and not just patch together other people’s code from the web, so I took a crash course in Python (using the excellent book Python Crash Course by Eric Matthes) and became familiar with C and C++ for the Arduino code.

After a few months of coding I’m finally at the point where it is all coming together. I’ve had the basic Lego robot built for months but never really programmed it to do anything other than running forward and stopping. The robot is built of parts from the Lego Crawler Crane kit (#42042), it’s basically the tracked base with some modifications like a motor for each track to allow steering. The motors are controlled via the Arduino (see my first blog entry), which in turn is controlled with a Python program on my PC via the serial monitor. The plan is to have a Raspberry Pi as the onboard PC, which in turn will be controlled via Wifi from another PC or laptop.

At the moment the robot is controlled with a simple Python program using the WASD keyboard keys, allowing forward and backward movement and turning. This works relatively well, with about a 1 second delay.

I took the video below when testing the serial communications; at that point I had only programmed forward movement.

I have attached the Arduino code below. It waits for a serial command to actuate the motors. If you intend to use this code yourself, be aware that some lines may be split because of the narrow WordPress theme format (should be fixed by now).

// Tank movement controlled via serial commands
// Works with Adafruit Motor Shield v.2.3
// Written by Kenneth Larsen, 2017

#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"

// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();

// Select ports for motors: M1 - M4. 
Adafruit_DCMotor *Motor1 = AFMS.getMotor(1);
Adafruit_DCMotor *Motor2 = AFMS.getMotor(2);

String input; // input for Serial
int c = -20; // error correction
int FULLSPEED = 255;
int HALFSPEED = 126;

void setup()
 {
 Serial.begin(115200);
 AFMS.begin(); // create with the default frequency of 1,6KHz
 }

void moveForward()
// Define forward movement
{
 Serial.println("Moving forward...");
 uint8_t i;
 Motor1->run(FORWARD);
 Motor2->run(FORWARD);
 for ( i = 30 ; i < 255 ; i++ ) // accelerate slowly
 {
 Motor1->setSpeed(i);
 Motor2->setSpeed(i+c); 
 delay(10);
 }
 Motor1->setSpeed(FULLSPEED); // maintain max speed
 Motor2->setSpeed(FULLSPEED+c);
}

void moveBackward()
// Define backward movement
{
 Serial.println("Moving backward...");
 uint8_t i;
 Motor1->run(BACKWARD);
 Motor2->run(BACKWARD);
 for ( i = 30 ; i < 255 ; i++ ) // accelerate slowly
 {
 Motor1->setSpeed(i);
 Motor2->setSpeed(i+c); 
 delay(10);
 }
 Motor1->setSpeed(FULLSPEED); // maintain max speed
 Motor2->setSpeed(FULLSPEED+c);
}

void turnLeft()
// Define left turns
{
 Serial.println("Turning left...");
 Motor1->run(BACKWARD);
 Motor2->run(FORWARD);
 Motor1->setSpeed(HALFSPEED);
 Motor2->setSpeed(HALFSPEED);
}

void turnRight()
// Define right turns
{
 Serial.println("Turning right...");
 Motor1->run(FORWARD);
 Motor2->run(BACKWARD);
 Motor1->setSpeed(HALFSPEED);
 Motor2->setSpeed(HALFSPEED);
}

void stopMovement()
// Define stopping
{
 Serial.println("Stopping...");
 Motor1->run(RELEASE); // stop motors
 Motor2->run(RELEASE);
}

void loop()
{
 if (Serial.available() > 0)
 {
 input = Serial.readString();
 if (input == "forward")
 {
 moveForward();
 }
 if (input == "backward")
 {
 moveBackward();
 }
 if (input == "left")
 {
 turnLeft();
 }
 if (input == "right")
 {
 turnRight();
 }
 if (input == "stop")
 {
 stopMovement();
 }
 }
}

The Python code uses pygame to grab the keystrokes and send them to the Arduino via serial. I’ll split this program into two, one for the PC to send the commands and receive video, and another for the Raspberry Pi to forward the commands to the Arduino and send video directly from the Raspicam to the PC. This will be done over WiFi to make the robot completely cordless, and with better range than if I went with Bluetooth (and better security!).

# Serial control for Arduino robot
# Written by Kenneth Larsen, 2017
import serial, sys
import pygame

# Initialize pygame
pygame.init()
pygame.display.set_caption("Robot Control Center")
screen = pygame.display.set_mode((640,480))

# Initialize serial connection
try:
 ser = serial.Serial('/dev/ttyACM1', 115200)
except serial.serialutil.SerialException:
 try:
 ser = serial.Serial('/dev/ttyACM0', 115200)
 except serial.serialutil.SerialException:
 print("No serial connection found. Is the Arduino connected?\n")

# Define stop keys. Releasing any of these keys will cause the robot to stop
stopkeys = (pygame.K_w, pygame.K_a, pygame.K_s, pygame.K_d)

def quitprogram():
 """Handles program shut down."""
 print("Stopping robot and exiting...\n")
 # Stop motors
 ser.write(b'stop')
 sys.exit()

# Main loop
while True:
 screen.fill((255,255,255))
 for event in pygame.event.get():
 if event.type == pygame.QUIT:
 quitprogram()
 # Keyboard controls
 if event.type == pygame.KEYDOWN:
 if event.key == pygame.K_q:
 quitprogram()
 if event.key == pygame.K_w:
 ser.write(b'forward')
 print(ser.readline().decode('UTF-8'))
 if event.key == pygame.K_s:
 ser.write(b'backward')
 print(ser.readline().decode('UTF-8'))
 if event.key == pygame.K_a:
 ser.write(b'left')
 print(ser.readline().decode('UTF-8'))
 elif event.key == pygame.K_d:
 ser.write(b'right')
 print(ser.readline().decode('UTF-8'))
 elif event.type == pygame.KEYUP:
 if event.key in stopkeys:
 #if event.key == pygame.K_w or pygame.K_s or pygame.K_a or pygame.K_d:
 ser.write(b'stop')
 print(ser.readline().decode('UTF-8'))
 
 # Update display
 pygame.display.update()

This is just the initial programming of the robot. I’ve tried to keep everything as simple as possible to assure that it all works.

Next step will be to get the Raspberry Pi on the robot and connect the Arduino directly to it. I’m currently experimenting with sending live video from the Raspberry over Python. The next update will hopefully focus on this, with the robot controlled over WiFi from a laptop.

Part 2 is now available!