Tuesday, 27 November 2012

Raspberry Pi Powered Lego Car


Want to take your Raspberry Pi some old Lego and build a remote control Lego car?  Well read on as this is the place to find out how to do it!

[Note: This was my original, 2012 era blog post on my Raspberry Pi car.  Read on for basic details as to how I made it.  See here for my 2013 post on how to upgrade it!] 

Two things have happened recently that have contributed to my tinkering hobby.
  • Firstly - My Raspberry Pi arrived after a long (but worth it) wait.
  • Secondly - My two girls (8 and 5) have got in to Lego and so slowly but surely I've been bringing down old Lego that I had when I was a kid, got stored in my parent's loft when I grew up and then got transferred to my loft when my parents had a general chuck out.
The coolest Lego model I had when I was a kid was a the Lego 8860 car chassis.  I brought this down from my loft, dusted it off and over the course of 3 weekends my daughters and I built it.  Here's a picture of it after we'd finished:
The car has lots of great features like rack and pinion steering, movable/reclining seats, a gear box, a differential, rear suspension and even a engine with pistons.

I'm very proud of my childhood self as all the parts were pretty much still there.  A few missing parts were easily replaceable with bits from other sets.  

The only missing part that I couldn't replace was a tiny bevel sprocket from the differential.  However (as I learned) there's a sites on the internet where you can buy old bits.  I used this site for the missing sprocket and for some other stuff I needed (see below).

The final pages of the assembly guide show how you can modify the car to add motors.  I never did this as a kid as I didn't have any Lego motors.  However as an adult with a bit more disposable income and some eager children I bought a few Lego power functions motors (again from the site mentioned above).  They were OK but required a battery box to be hard wired to them that meant you had to crawl along next to the car as it went along which wasn't much fun.

This is where the Raspberry Pi came in.  What if we could control the whole thing with the Raspberry Pi and make a remote control car!  That would be very cool!

Here's a video of the end result.  I'll then tell you how we made it!





A bit of research on the Raspberry Pi forum and other sites showed that people had been able to use the GPIO on the Raspberry Pi to control motors.  However we needed to start off by working out how to use  the Pi to control stuff with the GPIO.

We started off by looking at some simple electronics and Python code to use the GPIO to turn an LED off and on. We used this tutorial to get us started, used a breadboard, and LED and a resistor and wrote some Python code to control the LED.  Here's what it looked like:

Here you can see the Raspberry Pi, main connecting cables and some jumper wires going to a bread board which has an LED and a resistor on it.

GPIO11 goes to the anode on the LED.  The LED cathode connects to a resistor (~330Ohm) which then connects to GPIO6 (Ground). 




My Pi memory card came with Raspbian Wheezy on it and this already had Python installed.  We added a USB WiFi module (just visible on the image above) and configured this using the tool on the X windows desktop.

We created this simple Python script to turn the LED off and on:

#Get the time module
import time

#Get rid of warnings
GPIO.setwarnings(False)

#Set mode of numbering of pins
GPIO.setmode(GPIO.BOARD)

#Set pin 11 as an output
GPIO.setup(11,GPIO.OUT)

#Loop, changing the state
while 1:
  #Now set it high
  GPIO.output(11,True)

  #Wait a sec!
  time.sleep(1)

  #Now set it low
  GPIO.output(11,False)

  #Wait a sec!
  time.sleep(1)

It was called gpio_1.py so you type sudo python gpio_1.py to run the script.

So in researching motor control, the general advice was that motors require too much voltage and current to be driven directly from the Raspberry Pi.  The most common advice was to use a motor controller chip and this pre-made board seemed to have everything we needed.  So I bought one!

The board is based on a L298N motor controller and can be used to control a pair of motors.  Features:
  • A pair of inputs that can be controlled by TTL logic levels (like the Raspberry Pi uses).
  • Connections for a 9Volt battery.
  • A pair of outputs to connect to the DC motors.
So if input 1 is high and input 2 is low, the motor turns in one direction.  Input 1 low and input 2 high turns the motor in the other direction.  So for example the script shown above would make the motor turn for a second, pause for, turn for a second and so on.

Next challenge, connect the motor controller to the Lego motors:

The Lego power functions motors have stack-able connectors.  This site describes the motors in more detail and shows the pin out for the connectors.  

The two central pins are used to control the forward and back motion of "medium" motors that I bought.  The image shows how you can gently prize the pins of the connectors back and insert a single core wire. A modified (with a knife!) Lego tile holds the wires in place.

The image below shows the test jig for the Raspberry Pi, motor controller and the motors:

Here you can see the Raspberry Pi (on the left), 4 jumper wires going from the GPIO to the motor controller inputs, a jumper wire going to the motor controller ground, a battery connector and then the connections to the motors.  a script not unlike the one shown above was used to test that both motors worked.  They did!!




A (hand drawn - sorry) schematic for the system is shown below:

The next challenge was making the Raspberry Pi "mobile". We already had a WiFi USB adapter for IP connectivity but the Raspberry Pi needs a) power from a 5V micro USB supply and b) a connection to a TV or monitor via HDMI or S video.  To get over the power issue we bought a 5V re-chargeable battery; the sort you can get to re-charge a mobile 'phone.  We bought a Powergen 5200mAh re-chargeable unit which seemed like it would do the job for this but was also small enough to fit in a pocket which might be useful for other tinkering...

We couldn't have a hulking great cable linking the Pi to a monitor so I simply installed PuTTY on my PC and enabled SSH on the Raspberry Pi via the start-up utility.  PuTTY enables you to have a remote terminal session on the Pi and, by fixing the IP address on my WiFi router config. I always know which address to SSH to.

The last thing to sort out was connecting the motors to the Lego car. The motor to drive the wheels was easy enough, we just removed the engine parts and inserted one of the motors (as per 1980s instructions).  The steering was trickier as we needed to gear down the motor else it would just spin wildly and not have enough torque to turn the wheels.  To do this we built a little Lego gearbox:

From the image you can see that there's 3 pairs of small cog - medium cog pairings which gears down the motor speed nicely.  It needs a bit of a tidy up but it does the job for now.
   






So all the bits were ready, we just needed to assemble the car and write some Python code to control the motors.  Here's the car with all the kit stacked on it:

The seats had to be removed but the RasPi plus battery sit nicely on one side and the motor controller and it's battery sit nicely on the other side.








The first script I wrote was this:

#These are the keyboard mappings
#q = Go forward
#a = Stop going forward or back
#z = Go back
#i= Go left
#o = Stop steering
#p = Go right

#Get the GPIO module
import RPi.GPIO as GPIO

#Get the time module
import time

#A routine to control a pair of pins
def ControlAPairOfPins(FirstPin,FirstState,SecondPin,SecondState):
  print "Controlling them pins"
  if FirstState == "1":
    GPIO.output(int(FirstPin),True)
  else:
    GPIO.output(int(FirstPin),False)

  if SecondState == "1":
    GPIO.output(int(SecondPin),True)
  else:
    GPIO.output(int(SecondPin),False)
  #Just retur
  return

####Main body of code

#Get rid of warnings
GPIO.setwarnings(False)

#Set the GPIO mode
GPIO.setmode(GPIO.BOARD)
#Set the pins to be outputs
GPIO.setup(11,GPIO.OUT)
GPIO.setup(13,GPIO.OUT)
GPIO.setup(19,GPIO.OUT)
GPIO.setup(21,GPIO.OUT)

while True:
  MyChar = raw_input("Press a character:")
  print "You pressed: " + MyChar
  if MyChar == "q":
    ControlAPairOfPins("19","1","21","0")
    print "Forward"
  elif MyChar == "a":
    ControlAPairOfPins("19","0","21","0")
    print "Stop"
  elif MyChar == "z":
    ControlAPairOfPins("19","0","21","1")
    print ("Back")
  elif MyChar == "i":
    ControlAPairOfPins("11","1","13","0")
    print "Left"
  elif MyChar == "o":
    ControlAPairOfPins("11","0","13","0")
    print "Stop steering"
  elif MyChar == "p":
    ControlAPairOfPins("11","0","13","1")
    print "Right"
  else:
    print "Not a command"

So we used the keyboard (via SSH) to control the motors.  q,a,z made the car go forward, stop and back, (look at how they line up on a keyboard).  i,o,p made the car go left, stop steering and right (again look at the keyboard).  the raw_input() method is used to capture what the user types on the keyboard.  This is then mapped to the GPIO pins and whether they need to be high or low.  The ControlAPairOfPins() function is then called to set the state of the GPIO pins.  The code is a bit clunky but it worked and enabled us to control the motors via the SSH session.  The main downside was that raw_input()requires you to type enter after the character meaning it's a bit of a pain to remember the character, type it, press enter etc.

So we needed a better solution and this came from my old friend SL4A, (see previous posting).  Using this we could get an Android handset to communicate with the Raspberry Pi and control the motors.  SL4A gives access to all the Android sensors so we decided to use the position sensor, meaning we could tilt the handset forward and back (to make the car go forward and back) and left and right (to steer left and right).  a crazy idea but it just might work! 

First we needed to think about how the Android handset would communicate with the RasPi.  For my LightwaveRF kit I've sent command in UDP segments so I decided to do that again, (i.e. send from handset to Raspberry Pi using my home WiFi).  The protocol I decided on was simple.  Rather than typing q,a,z,i,o,p on the keyboard I would send this in UDP segments from the handset to the RasPi. The code to do this is below.  All I did was to import the socket module and put a socket listener within a while loop (rather than raw_input().  If the UDP socket receives a character then this is used to control a pair of pins.

#These are the keyboard mappings
#q = Go forward
#a = Stop going forward or back
#z = Go back
#i= Go left
#o = Stop steering
#p = Go right

#Get the GPIO module
import RPi.GPIO as GPIO

#Get the time module
import time

#Get the socket module
import socket

#Some IP constants for this, the server
UDP_IP = "192.168.0.5"
UDP_PORT= 8888

#A routine to control a pair of pins
def ControlAPairOfPins(FirstPin,FirstState,SecondPin,SecondState):
  print "Controlling them pins"
  if FirstState == "1":
    GPIO.output(int(FirstPin),True)
  else:
    GPIO.output(int(FirstPin),False)

  if SecondState == "1":
    GPIO.output(int(SecondPin),True)
  else:
    GPIO.output(int(SecondPin),False)
  #Just retur
  return

####Main body of code

#Get rid of warnings
GPIO.setwarnings(False)

#Set the GPIO mode
GPIO.setmode(GPIO.BOARD)
#Set the pins to be outputs
GPIO.setup(11,GPIO.OUT)
GPIO.setup(13,GPIO.OUT)
GPIO.setup(19,GPIO.OUT)
GPIO.setup(21,GPIO.OUT)

#Set up the IP related details
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP
sock.bind((UDP_IP, UDP_PORT))

#Tell the user we've started
print "UDP server started.  Waiting for response...."

while True:
  #Wait for a UDP command to be received
  print "Waiting for a UDP command"

  #Handles getting a UDP packet
  MyChar, addr = sock.recvfrom(1024) #buffer size is 1024

  #MyChar = raw_input("Press a character:")

  print "I received: " + MyChar

  if MyChar == "q":
    ControlAPairOfPins("19","1","21","0")
    print "Forward"
  elif MyChar == "a":
    ControlAPairOfPins("19","0","21","0")
    print "Stop"
  elif MyChar == "z":
    ControlAPairOfPins("19","0","21","1")
    print ("Back")
  elif MyChar == "i":
    ControlAPairOfPins("11","1","13","0")
    print "Left"
  elif MyChar == "o":
    ControlAPairOfPins("11","0","13","0")
    print "Stop steering"
  elif MyChar == "p":
    ControlAPairOfPins("11","0","13","1")
    print "Right"
  else:
    print "Not a command"

Looking at the handset side of things, we needed to learn how to detect the state of the handset sensors.  This simple tutorial shows how to do it.  The tutorial shows how to read all the sensors (magnometer, accelerometer, position sensor, light sensor) but I only wanted to use the position sensor. Running a line of code like this  s6 = droid.sensorsReadOrientation().result gives a result with 3 values which are [azimuth, pitch, roll].  From experimentation we observed that tilting the handset forward and back changed the pitch value and side to side changed the roll value (like an aeroplane I guess).

So all the Python script needed to do was read the position, check it against some thresholds and determine if it had changed.  If it had changed then I just needed to send the associated letter in a UDP segment.  here's the code:


#All the import statemets
import socket           #For UDP
import time             #For time
import android          #Android environment

#Our Droid entity
droid = android.Android()

#Some constants related to IP
UDP_IP = '192.168.0.5'
UDP_PORT = 8888
INET_ADDR = (UDP_IP,UDP_PORT)

#Some constants relating to direction.  These come from what I originally did on a keyboard
MoveForward= "q"
MoveBack = "z"
MoveStop = "a"
SteerLeft = "i"
SteerRight = "p"
SteerStop = "o"

#Some constants relating to thresholds.  Can be used to tune sensitivity
ForwardThresh = 0.3     #> This means you're tilted forward
BackwardThresh = -0.3   #< This means you're tilted backwards
LeftThresh = -0.4       #< This means you're tilted left
RightThresh = 0.4       #> This means you're tilted right 

######################################
#Main part of the code
######################################

#Initialise the movement and steering state
MoveState = MoveStop
SteerState = SteerStop

#Use these variables to get the current state
CurrentMove = ""
CurrentSteer= ""

#Print port information
print "UDP target IP:", UDP_IP 
print "UDP target port:", UDP_PORT 

#Set up the socket
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

#Start sensing the Android sensors. Has a timer to allow the sensors to spark up
droid.startSensingTimed(1, 250)
time.sleep(1)

#Loop, detecting state and sending UDP
while True:
  #Use a try statement and add some error handling as well
  try:  
    #Read the sensor state     
    s1 = droid.sensorsReadOrientation().result  
    
    #The result is a list made up of float variables.  Presented as a CSV on screen but we can mathematically manipulate
        
    #Set up the current states based upon the thresholds defined as constants - First for forward and back
    if s1[1] > ForwardThresh:
      CurrentMove = MoveForward
    elif s1[1] < BackwardThresh:
      CurrentMove = MoveBack
    else: CurrentMove = MoveStop

    #Now do it for left and right
    if s1[2] > RightThresh:
      CurrentSteer = SteerRight
    elif s1[2] < LeftThresh:
      CurrentSteer = SteerLeft
    else: CurrentSteer = SteerStop

    #So we've got the current state, now check with the overall logged state.  If it's changed, we do some UDP fun
    #First check the forward / back state
    if CurrentMove == MoveState:
      #Do nothing
      CurrentMove = CurrentMove    #Seems necessary other you get an error
    else:
      #Now we need to send UDP and update the overall state
      MoveState = CurrentMove
      sock.sendto(CurrentMove, INET_ADDR)
      print "Forward / back state changed.  Sent: " + CurrentMove      
    
    #Now the left / right state
    if CurrentSteer == SteerState:
      #Do nothing
      CurrentSteer = CurrentSteer    #Seems necessary other you get an error 
    else:
      #Now we need to send UDP and update the overall state
      SteerState = CurrentSteer
      sock.sendto(CurrentSteer, INET_ADDR)      
      print "Left / right state changed.  Sent: " + CurrentSteer
   
      #Chill for a sec
      time.sleep(1)
  except Exception, err: #Handle exceptions
    #Write a screen with the error
    print "Got us an exception: " + str(err)   
    time.sleep(1)


As the video shows it works OK.  It needs tuning to speed the car up, change the tilt sensitivity on the handset but overall it's good.  We now need to work out other ways of controlling the car with the Raspberry Pi.