Raspberry Pi Powered Lego Car 2.0

Want to take a Raspberry Pi, some Lego motors and a Wii Controller and build a remote control car?  Well read on as this page will tell you how to do it!!

Last year my daughters and I built a Lego car that, with the addition of a Raspberry Pi, some motors and a motor controller we could control using an Android mobile phone.

Having bought a Bluetooth dongle for another project (hopefully more about that in a later post), a random meander around the internet showed me I could link the Raspberry Pi to a Wii remote control (Wiimote) and do "stuff".  Hence I had the idea to control the Lego car with a Wii remote plus the Wii steering wheel you buy for things like Mario Kart Wii.

Here's a video of the final car.  Read on below to see how I made it.

The basic building blocks:
-Lego motors + Raspberry Pi + Motor controller.  Lots of detail on the post I wrote last year.
-A Bluetooth dongle, bought from modmypi.
-Details on how to integrate a Wiimote to a Linux machine using cwiid.  There's a great example of using it on the Cambridge University website.

Feedback I had from my daughters last year was that the old school Lego car we used was too slow (because it was heavy) and too difficult to steer.  Hence I decided to build a new, much smaller and lighter car with a different steering mechanism.

From a steering perspective, what was wrong last year was that the rack and pinion steering was simply too cumbersome.  To turn right you steered right.  Then to straighten up you had to steer left just enough to straighten up the wheels.  At this time you generally were not moving the car forward or back because it was too confusing.  All-in-all it was all too finicky so I decide to use a tank track style steering mechanism because this meant that you could send a simple "steer-right" or "steer left" command and it would only turn for the period you issue the command.

Using cwiid is very easy:
-Install on a Pi using the command sudo apt-get install python-cwiid.
-Import the library with import cwiid and create an object to access Wiimote actions using wm = cwiid.Wiimote() 
-Report button presses with wm.rpt_mode = cwiid.RPT_BTN and check the actual state with statements like if (wm.state['buttons'] & cwiid.BTN_1):
-Report the accelerometer state with wm.rpt_mode =  cwiid.RPT_ACC and get the actual state with  print(wm.state['acc'])

The accelerometer state is a tuple with 3 values.  When holding the Wiimote in the Wii steering wheel, it's the second value that varies if you steer left and right.  So doing:

wm.rpt_mode = cwiid.RPT_ACC
AccVar =  wm.state['acc']
...means that you can assess the left write position of the wheel by assessing AccVar[1].  With he Wiimote horizontal the value was 120.  It increased as I "steered" left and decreased as I "steered" right.  Hence I decided to set the steer left threshold as > 130 and the steer right threshold as < 110 which was roughly equal to the 10 o clock and 2 o clock positions.

So after playing with cwiid, button pressing and accelerometer reading I set about building the car. Here's a few more images:

I can't publish a full "how-to-build" guide as I made it up as I went along!  However here's a few highlights:
-Built on two levels; motors, tracks + wheels and Pi battery on the bottom layer, the Pi, motor controller board and motor battery on the top level.
-Two motors mounted at 90 degrees to the direction that the tank track turns.  I used a pair of cogs that allow motion to be transferred through 90 degrees.  These connected to a cog of equal size that drove the tank track.  I experimented with software PWM but found that with this gearing and the weight of the car I could simple switch the motors off and on and the car would move.  (I've left some of the PWM in the full code listing below so use it if you wish).
-Some tank tracks mounted on large cogs.  The tank tracks came from a Lego pneumatic excavator that I owned as a child. I did try various wheel arrangements but found it would only work with the tracks.
-On the top level, a careful arrangement of Raspberry Pi, motor controller board and motor battery.  For the battery I used an 7.2V, 5000mAh battery from an old camcorder.  This gives much more "play time" than the old PP3 re-chargeable I previously used.
-Various odds and ends to keep everything in place and stop the wires from falling out or snagging in the tracks.

Control of the tracks via the motors was very easy.  There is basically 5 ways to drive the motors:
-Left hand motor (LHM) clockwise, right hand motor (RHM) antiwise = Go forward.
-LHM anticlockwise, RHM clockwise = Go backward.
-LHM and RHM clockwise = Turn left.
-LHM and RHM anti-clockwise = Turn right.
-Motors off, stopped (!).

The code is basically a state machine that assesses which buttons are pressed and how the Wiimote is tilted, (e.g. button 1 pressed and Wiimote value between 110 and 120 means the state is ForwardNoSteer).  If the state does not change then no action is taken, if the state changes then the motors are command to move appropriately.

Full code listing below.  Next steps: do something with the Raspberry Pi camera and control the car with Scratch!

#Mostly used code from these two sites
#https://code.google.com/p/raspberry-gpio-python/wiki/PWM
#http://www.cl.cam.ac.uk/projects/raspberrypi/tutorials/robot/wiimote/

import cwiid 
import time
import os
import RPi.GPIO as GPIO   #The GPIO module

#A routine to control the pins
def ControlThePins(TheState):
  print "Controlling them pins:c" + TheState 
  #First just turn them all off - then we work out what to do
  GPIO.output(11,False)
  GPIO.output(13,False)
  GPIO.output(19,False)
  GPIO.output(21,False)

  #Just work through the different motor states, setting the pins accordingly
  if TheState == GoForward:
    GPIO.output(11,True)
    GPIO.output(13,False)
    GPIO.output(19,True)
    GPIO.output(21,False)
  elif TheState == GoBackward:
    GPIO.output(11,False)
    GPIO.output(13,True)
    GPIO.output(19,False)
    GPIO.output(21,True)
  elif TheState == SteerLeft:
    GPIO.output(11,True)
    GPIO.output(13,False)
    GPIO.output(19,False)
    GPIO.output(21,True)
  elif TheState == SteerRight:
    GPIO.output(11,False)
    GPIO.output(13,True)
    GPIO.output(19,True)
    GPIO.output(21,False)
  elif TheState == StopIt:
    #Do nothing as we stopped all pins first thing
    pass

  #Just return
  return

#This is a PWM routine.  
def ControlMyPins(Num11,Num13,Num19,Num21):
  print "PWM Control of pins using " + Num11 + "-" + Num13 + "-" + Num19 + "-" + Num21

  if Num11 == "1":
    #Speed the motor up
    for dc in range(0, 101, 5):
      PIN11.ChangeDutyCycle(dc)
      time.sleep(0.03)
  else:
      #Stop PWM
      PIN11.start(0)  

  if Num13 == "1":
    #Speed the motor up
    for dc in range(0, 101, 5):
      PIN13.ChangeDutyCycle(dc)
      time.sleep(0.03)
  else:
    #Stop PWM
    PIN13.start(0)  

  #And return
  return

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

#Set up the GPIO pins
#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 the pins to be PWM pins 
#PIN11 = GPIO.PWM(11, 100)  # channel=11 frequency=50Hz
#PIN11.start(0)
#PIN13 = GPIO.PWM(13, 100)
#PIN13.start(0)
#PIN19 = GPIO.PWM(19, 100)
#PIN19.start(0)
#PIN21 = GPIO.PWM(21, 100)
#PIN21.start(0)

#Now we start defining some constants to use.  We have three concepts:
#1)The motor command.  Stating Forward, Backward, Left, Right, Stop
#2)The sensor position and whether a button is pressed
#3)The system state, e.g. Sys1Level, Sys1Right etc
#We determine sensor position, see if the system state has changed and if so send a new motor command

#Motor commands
GoForward = "Forward"
GoBackward = "Backward"
SteerRight = "Right"
SteerLeft = "Left"
StopIt = "Stop"

#Constants for the system state and then an assignment
ForwardNoSteer = "FNS"
ForwardSteerLeft = "FSL"
ForwardSteerRight = "FSR"
BackwardNoSteer = "BNS"
BackwardSteerLeft = "BSL"
BackwardSteerRight = "BSR"
SystemStopped = "SS"
SystemState = SystemStopped

#Now left and right
#You get a 3 tuple and the middle value is what you need.
#Horizontal is 120.  Going right decreases the value so < 110 is steer right.  Going left increases the value so > 130 is steer left
SteerRightValue = 110
SteerLeftValue = 130

#Make the Bluetooth dongle discoverable
os.system("sudo hciconfig hci0 piscan")

#connecting to the Wiimote. This allows several attempts 
# as first few often fail. 
print 'Press 1+2 on your Wiimote now...' 

wm = None 
i=2 
while not wm: 
  try: 
    wm=cwiid.Wiimote() 
  except RuntimeError: 
    if (i>10): 
      print "Giving up connecting"
      quit() 
      break 
    print "Error opening wiimote connection" 
    print "attempt " + str(i) 
    i +=1 
    #Pause for a bit
    time.sleep(0.5)

#Got here, tell the user
print "Success - we have connected!"

#set Wiimote to report button presses and accelerometer state 
wm.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_ACC 

#Wait a bit
time.sleep(1)

#Do a celebratory LED KITT style sweep
LoopVar = 0
while LoopVar < 3: 
  #turn on leds to show connected 
  wm.led = 1
  time.sleep(0.1)
  wm.led = 2
  time.sleep(0.1)
  wm.led = 4
  time.sleep(0.1)
  wm.led = 8
  time.sleep(0.1)
  #Set up for next loop 
  LoopVar +=1  

#Turn off the LEDs now
wm.led = 0

#Wait a bit
time.sleep(0.5)

#Count in binary on the LEDs
#for i in range(16):
  #wm.led = i
  #time.sleep(0.5)

#Do a rumble
wm.rumble = True
time.sleep(0.5)
wm.rumble = False

#Now start checking for button presses
print "Ready to receive button presses and accelerometer input"

#Loop for ever detecting

try:
  while True:
    #Set up a button object to check
    buttons = wm.state['buttons']

    #We assess whether the Wiimote is level, left or right by assessing the accelerometer
    AccVar = wm.state['acc']
    
    #Check all the different possible states of button, accelerometer and system state.  First button 1 pressed and steering left
    if (buttons & cwiid.BTN_1) and (int(AccVar[1]) > SteerLeftValue) and (SystemState != ForwardSteerLeft):
      #Tell the user
      print "Forward Steer Left"

      #Change the state to be this now
      SystemState = ForwardSteerLeft
        
      #Set the motor state
      ControlThePins(SteerLeft)
    elif (buttons & cwiid.BTN_1) and (int(AccVar[1]) < SteerRightValue) and (SystemState != ForwardSteerRight):   #Button 1 pressed and steering right
      #Tell the user
      print "Forward Steer Right"

      #Change the state to be this now
      SystemState = ForwardSteerRight

      #Set the motor state
      ControlThePins(SteerRight)
    elif (buttons & cwiid.BTN_1) and ((int(AccVar[1]) >= SteerRightValue) and (int(AccVar[1]) <= SteerLeftValue)) and (SystemState != ForwardNoSteer):   #Button 1 pressed.  Acclerometer in the middle  
      #Tell the user
      print "Go forward"

      #Change the state to be this now
      SystemState = ForwardNoSteer

      #Set the motor state
      ControlThePins(GoForward)
    elif (buttons & cwiid.BTN_2) and (int(AccVar[1]) > SteerLeftValue) and (SystemState != BackwardSteerLeft):    #Backward and steering left
      #Tell the user
      print "Backward Steer Left"

      #Change the state to be this now
      SystemState = BackwardSteerLeft

      #Set the motor state
      ControlThePins(SteerLeft)
    elif (buttons & cwiid.BTN_2) and (int(AccVar[1]) < SteerRightValue) and (SystemState != BackwardSteerRight):   #Button 2 pressed and steering right
      #Tell the user
      print "Backward Steer Right"

      #Change the state to be this now
      SystemState = BackwardSteerRight

      #Set the motor state
      ControlThePins(SteerRight)
    elif (buttons & cwiid.BTN_2) and ((int(AccVar[1]) >= SteerRightValue) and (int(AccVar[1]) <= SteerLeftValue)) and (SystemState != BackwardNoSteer):   #Button 2 pressed.  Acclerometer in the middle  
      #Tell the user
      print "Go backward"

      #Change the state to be this now
      SystemState = BackwardNoSteer

      #Set the motor state
      ControlThePins(GoBackward)
    #No button pressed so we reach this else statement, see if it's because of a change of state  
    elif (not(buttons & cwiid.BTN_1)) and (not(buttons & cwiid.BTN_2)) and (SystemState != SystemStopped):  
      #Tell the user
      print "No buttons pressed"
         
      #Change the state to be this now
      SystemState = SystemStopped
          
      #Change the motor state to be off
      ControlThePins(StopIt)
      
    #Chill for a bit
    time.sleep(0.1)
except KeyboardInterrupt:
    pass

print "Good night"

#End of the code - turn off pins then exit
ControlThePins(StopIt)