Having received my Raspberry Pi I was keen to do something interesting with it, hopefully spark some interest from my two boys. I’ve used it as an XBMC connected to the main TV as well as controlling LEDs. I came across the following two Blog posts and was keen to reproduce this myself; the boys seemed keen too. Note this sort of thing is completely new to me – I would have been completely lost without the information in the Blog articles below as well as the excellent tutorials on the Adafruit website.
Following these two blog posts I went ahead and purchased the following items:
The Adafruit 16-channel PWM driver is required as the Raspberry Pi only natively supports one PWM channel (on GPIO 18) – this project requires two (one for the engine and one for the steering servo). The Adafruit PWM board allows the Pi to communicate with 16 different PWM devices via the Pi’s built-in hardware I2C support.
The motor driver is for interfacing between PWM input (0 .. 4095 for motor speed) and the DC output as required by the Lego motor.
The next problem was power – it became apparent that I would need three separate power sources:
From the Adafruit servo tutorial:
Why not use the +5V supply on the Raspberry Pi? Switching directions on the servo can cause a lot of noise on the supply, and the servo(s) will cause the voltage to fluctuate significantly, which is a bad situation for the Pi. It's highly recommended to use an external 5V supply with servo motors to avoid problems caused by voltage drops on the Pi's 5V line.
Note the Lego 8881 9v Battery Box (6xAA) may have been a better choice as it is cheaper and holds AA batteries.
To help with the wiring (especially for when wires got accidently pulled out) I created the following wiring diagram using the excellent modelling tool Enterprise Architect from Sparx Systems (you can tell what I do for a living).
My first attempt was with the Lego Technic Dune Buggy (8048). The engine and servo actually fitted very nicely into the model; however it soon became apparent that there was going to be no way to fit the three power sources. This led to a temporary loss of interest and put the project on hold for six months.
Initially I had no particular plans for controlling the car – I saw that as the least of my worries; something to do once I’d got all of the other bits. When discussing the project with work colleagues they suggested controlling the car with a PS3 controller; I had no idea that they actually used Bluetooth assuming Sony would have instead used a proprietary protocol.
As the Pi doesn’t have built-in Bluetooth I went ahead and bought a Plugable USB Bluetooth 4.0 micro adapter from Amazon. While not on the list of verified Bluetooth adapters I had no problem getting this to work – really was just a matter of plugging it in.
There are a number of excellent articles for getting the PS3 Dualshock 3 controller working with Linux. These two are quite similar and specific to the Raspberry Pi:
I have an outstanding issue in that the PS3 controller doesn’t vibrate to indicate that sixad has connected, any tips on how to fix this would be much appreciated. The confusing thing is that the controller does get registered (as /dev/input/js0). I’m sure I had this working previously before having to rebuild the Linux installation (SD card corruption following power failure, oops).
As can be seen from the final Python code below, the pygame joystick library makes interfacing with the controller extremely easy.
We had an old Lego 4WD 8435 built and hanging around gathering dust on a shelf that we had forgotten about – we instantly decided that this would be perfect for the project as it looked like it had plenty of space for all of the components. We had to pull out the piston in the engine bay as well as the seats and both pulleys – once they were gone everything fitted in surprisingly well- the Anker battery in particular was a perfect fit.
Both engine and servo mounting required significant reinforcement around the mounting point as they both were pulling the model apart.
Obligatory YouTube video:
Photos
Living in the UK, I managed to source the majority of the technical components from the UK Adafruit reseller, SK Pang electronics; I have found them to be very reliable.
All of the Lego items were purchased direct from the Lego shop.
Thanks to both Tom Rees and Paul Weeks for the inspiration for this project. Please do let me know if there is anything I have got wrong or things that could be improved – as stated, this is all new to me.
I've been cleaning up the code a little and haven't yet tested this version - apologies if there are bugs, will check ASAP.
#!/usr/bin/python
import RPi.GPIO as GPIO
from Adafruit_PWM_Servo_Driver import PWM
import time
import signal
import sys
import pygame
import os
# Constants
FORWARD_GPIO = 17
BACKWARD_GPIO = 21
PWM_MAX = 2**12-1 # 12bits -> 0..4095
MOTOR_PWM = 0
STEERING_SERVO_PWM = 15
LIGHT1_GPIO = 23
LIGHT2_GPIO = 24
TEST_SERVO_ON_START = False
TEST_LEDS_ON_START = False
JS_STEERING_AXIS = 0
JS_ENGINE_AXIS = 3
JS_LIGHT1_BUTTON = 14
JS_LIGHT2_BUTTON = 15
JS_SHUTDOWN_BUTTON = 13 # TODO Need to check what button this is!
SHUTDOWN_BUTTON_PRESS_TIME = 4 # In seconds
# Max switching frequency of the TB6612FNG motor driver is 100KHz
# Adafruit PWM driver - Adjustable frequency PWM up to about 1.6 KHz
# Analog servos typically run at ~60 Hz updates
PWM_FREQUENCY = 60
# TowerPro SG-5010
# weight- 38g
# Dimension 40.2*20.2*43.2mm
# Stall torque 5.5kg/cm(4.8V); 6.5kg/cm(6V);
# Operating speed 0.2sec/60degree(4.8v); 0.16sec/60degree(6v)
# Operating voltage 4.8-6V
# Temperature range 0..55 deg C
# Dead band width 10us
# Position "0" / middle (1.5ms pulse), "90" / right (~2ms pulse), "-90" / left (~1ms pulse)
# Servo calibration values
SERVO_MIN = 230 # Min pulse length out of 4096 (1ms pulse @ 60Hz = 245)
SERVO_MAX = 490 # Max pulse length out of 4096 (2ms pulse @ 60Hz = 491)
SERVO_MID = (SERVO_MIN + SERVO_MAX) / 2 # (1.5ms pulse @ 60Hz = 368)
SERVO_RANGE = SERVO_MAX - SERVO_MID
# Initialise the PWM device using the default address
pwm = PWM(0x40, debug=True)
def signalHandler(signal, frame):
print("You pressed Ctrl-C")
cleanUpAndExit()
# Register a signal handler for safe clean-up
signal.signal(signal.SIGINT, signalHandler)
def setEngineSpeed(value):
"""Set engine motor speed
Arguments:
value -- range from -1 (forwards) to 1 (backwards)
"""
speed = min(int(abs(value)*PWM_MAX), PWM_MAX)
forward = value <= 0
print("setEngineSpeed(%f), forward=%i, speed=%f)" % (value, forward, speed))
if (forward):
GPIO.output(BACKWARD_GPIO, False)
GPIO.output(FORWARD_GPIO, True)
else:
GPIO.output(BACKWARD_GPIO, True)
GPIO.output(FORWARD_GPIO, False)
setPWM(MOTOR_PWM, 0, speed)
def setSteering(value):
"""Change the steering servo position
Arguments:
value -- range from -1 to 1 for -90 to 90 degrees movement
"""
servo_pos = int(SERVO_MID + SERVO_RANGE*value)
print("setSteering(%f), setting servo pos to %i" % (value, servo_pos))
setPWM(STEERING_SERVO_PWM, 0, servo_pos)
def setServoPulse(channel, pulse):
"""Set the pulse length in seconds
E.g. setServoPulse(0, 0.001) sets a ~1 millisecond pulse width. It's not precise
Good site on PWM - http://ebldc.com/?p=48. Note T-OFF in that article is different to that used by the Adafruit library
Arguments:
channel -- the PWM channel (0..15)
pulse -- pulse length in seconds
"""
pulseLength = 1000000 # 1,000,000 us per second
pulseLength /= PWM_FREQUENCY # e.g. 60 Hz
print("%d us per period" % pulseLength)
pulseLength /= PWM_MAX+1
print("%d us per bit" % pulseLength)
pulse *= 1000 # Convert to micro-sec
pulse /= pulseLength
setPWM(channel, 0, pulse)
def setPWM(channel, on, off):
"""Sets the start (on) and end (off) of the high segment of the PWM pulse on a specific channel
Note on + off must be < 4095
Arguments:
channel -- The PWM channel (0..15)
on -- when the signal should transition from low to high (12bits hence 4096). Valid values are 0..4095
off -- when the signal should transition from high to low (12bits hence 4096). Valid values are 0..4095
"""
pwm.setPWM(channel, on, off)
def testServo():
# Change speed of continuous servo on channel O
print("Setting servo to mid")
setPWM(STEERING_SERVO_PWM, 0, SERVO_MID)
time.sleep(1)
print("Setting servo to min")
setPWM(STEERING_SERVO_PWM, 0, SERVO_MIN)
time.sleep(1)
print("Setting servo to max")
setPWM(STEERING_SERVO_PWM, 0, SERVO_MAX)
time.sleep(1)
print("Setting servo to mid")
setPWM(STEERING_SERVO_PWM, 0, SERVO_MID)
def testLEDs():
gpio = LIGHT1_GPIO
print("Testing GPIO", gpio)
print("LED on for GPIO", gpio)
GPIO.output(gpio, True)
time.sleep(1)
print("LED off for GPIO", gpio)
GPIO.output(gpio, False)
time.sleep(1)
gpio = LIGHT2_GPIO
print("Testing GPIO", gpio)
print("LED on for GPIO", gpio)
GPIO.output(gpio, True)
time.sleep(1)
print("LED off for GPIO", gpio)
GPIO.output(gpio, False)
time.sleep(1)
def init():
# Set frequency in Hz (range is 40 to 1000)
pwm.setPWMFreq(PWM_FREQUENCY)
# Initialise the GPIO output channels
GPIO.setmode(GPIO.BCM)
GPIO.setup(FORWARD_GPIO, GPIO.OUT)
GPIO.setup(BACKWARD_GPIO, GPIO.OUT)
GPIO.setup(LIGHT1_GPIO, GPIO.OUT)
GPIO.setup(LIGHT2_GPIO, GPIO.OUT)
setEngineSpeed(0)
setSteering(0)
pygame.init()
pygame.joystick.init()
j = pygame.joystick.Joystick(0)
j.init()
def mainLoop():
init()
if (TEST_SERVO_ON_START): testServo()
if (TEST_LEDS_ON_START): testLEDs()
sd_button_down_time = None
while (True):
for event in pygame.event.get():
if (event.type == pygame.QUIT):
break
elif (event.type == pygame.JOYBUTTONDOWN):
if (event.button == JS_LIGHT1_BUTTON):
GPIO.output(LIGHT1_GPIO, True)
elif (event.button == JS_LIGHT2_BUTTON):
GPIO.output(LIGHT2_GPIO, True)
elif (event.button == JS_SHUTDOWN_BUTTON):
sd_button_down_time = time.time()
elif (event.type == pygame.JOYBUTTONUP):
if (event.button == JS_LIGHT1_BUTTON):
GPIO.output(LIGHT1_GPIO, False)
elif (event.button == JS_LIGHT2_BUTTON):
GPIO.output(LIGHT2_GPIO, False)
elif (event.button == JS_SHUTDOWN_BUTTON):
if (sd_button_down_time != None and time.time() - sd_button_down_time > SHUTDOWN_BUTTON_PRESS_TIME):
# Windows
#os.system("shutdown")
# Linux
os.system("poweroff")
else:
sd_button_down_time = None
elif (event.type == pygame.JOYAXISMOTION):
if (event.axis == JS_STEERING_AXIS):
setSteering(event.value)
elif (event.axis == JS_ENGINE_AXIS):
setEngineSpeed(event.value)
cleanUpAndExit()
def cleanUpAndExit():
setEngineSpeed(0)
setSteering(0)
GPIO.cleanup()
pygame.joystick.quit()
sys.exit(1)
if __name__ == "__main__":
mainLoop()