How to use a PS3 Controller to move a USB Robot Arm on a Raspberry Pi
By Hiren Mistry
This program enables the user to control the robot arm via a PlayStation 3 Controller, through USB connection.
Requirements:
· PyUSB- This must be installed so the Python Code can interact with the USB Robot Arm. It can be downloaded from https://sourceforge.net/projects/pyusb/files/PyUSB%201.0/1.0.0/pyusb-1.0.0.tar.gz/download
· PyGame- This module is needed to receive input from the PS3 controller. It can be downloaded from http://www.pygame.org/download.shtml
How does it Work?
To receive the input from the controller PyGame is used. PyGame is a set of modules used for writing games and contains the necessary modules need to receive the PlayStation controller input.
The input consists of 3 major parts of the controller:
· Buttons- These have a Boolean on/off state
· Hats- These are digital inputs and return a 1 or 0. Hats tilt left/right or up/down.
· Axis- These are your standard joysticks and return a value between 1 and -1. One joystick has two axes- up/down and left/right.
For my input, I have decided to use a combination of Buttons and Axis (Joysticks) to control the Robot Arm.
Image: This image has been modified from http://www.console.maban.co.uk/device/ps3. License:https://creativecommons.org/licenses/by-sa/3.0/legalcode
Next, I found out which input is assigned to which number which will enable me to move the arm once I have mapped each input.
Here are some commands:
Command
|
Explanation
|
pygame.joystick.init()
|
Initialize pygame to read controllers
|
pygame.joystick.get_count()
|
Get 1 joystick, number 0
|
pygame.joystick.Joystick(0)
|
Assign the first controller
|
joystick.init()
|
Initialize the first controller for reading
|
joystick.get_numaxes()
|
Return the number of axes on the controller
|
joystick.get_axis(axis_number)
|
Get the analogue value (float) of the specified axis
|
pygame.joystick.Joystick.get_button(i)
|
Returns the current state of a joystick button.
|
pygame.joystick.Joystick.quit
|
This will uninitialize the Joystick.
|
Axis Number
0 = Left joystick left to right values -1.0 to 1
1 = Left joystick up to down values -1.0 to 1
2 = Right joystick left to right values -1.0 to 1
3 = Right joystick up to down values -1.0 to 1
The picture above illustrates the axis value for each axis number depending on the position of the joystick. For example, if the left joystick was in the bottom left direction, then the axis numbers and values would be ‘Axis Number 0 = -1, Axis Number 1 = 1’.
Button Number
Button
|
Number
|
Button
|
Number
|
SELECT
|
0
|
R2
|
9
|
L3
|
1
|
L1
|
10
|
R3
|
2
|
R1
|
11
|
START
|
3
|
TRIANGLE
|
12
|
D-PAD UP
|
4
|
CIRCLE
|
13
|
D-PAD RIGHT
|
5
|
CROSS
|
14
|
D-PAD DOWN
|
6
|
SQUARE
|
15
|
D-PAD LEFT
|
7
|
PLAYSTATION LOGO
|
16
|
L2
|
8
|
Axis numbers and Button numbers were both found using the code below. This code prints the axis values (from -1 to 1) and Button States (1 or 0) if they are pressed. I would then press a button/move an axis and analyse the numbers produced. These may be different depending on how your controller is setup so I would recommend running the code below and test which buttons are assigned to which numbers.
import pygame, sys, time #Imports Modules
from pygame.locals import *
pygame.init()#Initializes Pygame
pygame.joystick.init()
joystick = pygame.joystick.Joystick(0)
joystick.init()#Initializes Joystick
# get count of joysticks=1, axes=27, buttons=19 for DualShock 3
joystick_count = pygame.joystick.get_count()
print("joystick_count")
print(joystick_count)
print("--------------")
numaxes = joystick.get_numaxes()
print("numaxes")
print(numaxes)
print("--------------")
numbuttons = joystick.get_numbuttons()
print("numbuttons")
print(numbuttons)
print("--------------")
loopQuit = False
while loopQuit == False:
# test joystick axes and prints values
outstr = ""
for i in range(0,4):
axis = joystick.get_axis(i)
outstr = outstr + str(i) + ":" + str(axis) + "|"
print(outstr)
# test controller buttons
outstr = ""
for i in range(0,numbuttons):
button = joystick.get_button(i)
outstr = outstr + str(i) + ":" + str(button) + "|"
print(outstr)
for event in pygame.event.get():
if event.type == QUIT:
loopQuit = True
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
loopQuit = True
# Returns Joystick Button Motion
if event.type == pygame.JOYBUTTONDOWN:
print("joy button down")
if event.type == pygame.JOYBUTTONUP:
print("joy button up")
if event.type == pygame.JOYBALLMOTION:
print("joy ball motion")
# axis motion is movement of controller
# dominates events when used
if event.type == pygame.JOYAXISMOTION:
# print("joy axis motion")
time.sleep(0.01)
pygame.quit()
sys.exit()
When running this testing code, I would test each part of the remote separately so detecting the inputs is easier. For example to test the axis, I would comment out everything from '#test controller buttons' to '#print ("joy axis motion")' and run the program.
Building the program
I will require multiple commands to happen at the same time but only one command will be sent to the Robot Arm at once. Therefore, the python file will need to build the command, detecting a range of inputs and compile one output command. To build one command, the commands need to be combined from their binary format into one integer command (See ‘Basic Commands using a USB Robotic Arm with a Raspberry Pi’)
Here is the final program, ps3controller.py with explanations on each line.
import pygame #Import Modules
import usb.core
import time
pygame.init()#Initialize pygame
# Wait for a joystick
while pygame.joystick.get_count() == 0:
print 'waiting for joystick count = %i' % pygame.joystick.get_count()
time.sleep(10)
pygame.joystick.quit()
pygame.joystick.init()
j = pygame.joystick.Joystick(0)
j.init()#Initialize Joystick
print 'Initialized Joystick : %s' % j.get_name()#Print joystick if present
armFound = False
while not armFound: #Find Robot Arm
dev = usb.core.find(idVendor=0x1267, idProduct=0x0000)
if dev is None:#If Robot Arm not found, alert user
print 'Arm not found. Waiting'
time.sleep(10)
else:
armFound = True
#this arm should just have one configuration
dev.set_configuration()
#Print Controls to user
print("")
print("Joysticks:")
print("LEFT JOY UP/DOWN: SHOULDER UP/DOWN")
print("LEFT JOY LEFT/RIGHT: BASE CLOCKWISE/ANTICLOCKWISE")
print("RIGHT JOY UP/DOWN: ELBOW UP/DOWN")
print("R1: WRIST UP")
print("R2: WRIST DOWN")
print("X: GRIP OPEN")
print("O: GRIP CLOSE")
print("SELECT: TOGGLE LIGHT ON/OFF")
print("")
print("To Close, press 'Ctrl + C'")
# How far to move the JoyStick before it has an effect (0.60 = 60%)
threshold = 0.50#Sensitivity
# Key mappings
PS3_BUTTON_SELECT = 0
PS3_AXIS_LEFT_HORIZONTAL = 0
PS3_AXIS_LEFT_VERTICAL = 1
PS3_AXIS_RIGHT_HORIZONTAL = 2
PS3_AXIS_RIGHT_VERTICAL = 3
PS3_AXIS_X = 17
PS3_AXIS_CIRCLE = 18
PS3_AXIS_R1 = 15
PS3_AXIS_R2 = 13
# Robot Arm defaults
Command = (0,0,0)
lightonoff = 0
shoulder = 0
base = 0
elbow = 0
wristup = 0
wristdown = 0
grip_open = 0
grip_close = 0
grip_command = 0
wrist_command = 0
shoulder_command = 0
base_command = 0
elbow_command = 0
# ARM control
def SetCommand(axis_val):#Returns number depending on axis value
if axis_val > threshold:
return 1
elif axis_val < -threshold:
return 2
elif abs(axis_val) < threshold:
return 0
def BuildCommand(shoulc,basec,elbowc,wristc,gripc,lightc):#Builds Command
byte1 = shoulc + elbowc + wristc + gripc#Combines instructions for byte1
list1 = [str(shoulc), str(elbowc), str(wristc), str(gripc), str(basec), str(lightc)]#Compiles commands into 1 command
def ProcessArm(event):#Detects input and processes
global Command, lightonoff, shoulder, base, elbow, wristup, wristdown, grip_open, grip_close, grip_command, wrist_command, shoulder_command, base_command, elbow_command
if event.type == pygame.JOYBUTTONDOWN:
if event.button == PS3_BUTTON_SELECT:
if lightonoff == 0:
lightonoff = 1
else:
lightonoff = 0
elif event.type == pygame.JOYAXISMOTION:
if event.axis == PS3_AXIS_LEFT_VERTICAL:
shoulder = event.value
elif event.axis == PS3_AXIS_LEFT_HORIZONTAL:
base = event.value
elif event.axis == PS3_AXIS_RIGHT_VERTICAL:
elbow = event.value
elif event.axis == PS3_AXIS_R1:
wristup = event.value
elif event.axis == PS3_AXIS_R2:
wristdown = event.value
elif event.axis == PS3_AXIS_X:
grip_open = event.value
elif event.axis == PS3_AXIS_CIRCLE:
grip_close = event.value
# Open/Close Gripper?
if grip_open > threshold:
grip_command = 1
elif grip_close > threshold:
grip_command = 2
else:
grip_command = 0
# Wrist Up/Down?
if wristup > threshold:
wrist_command = 1*4
elif wristdown > threshold:
wrist_command = 2*4
else:
wrist_command = 0
#Produces final command for each 'body' part
shoulder_command = SetCommand(shoulder)*64
base_command = SetCommand(base)
elbow_command = SetCommand(elbow)*16
# Work out what to send out to the robot
NewCommand = BuildCommand(shoulder_command,base_command,
elbow_command, wrist_command, grip_command,lightonoff)
# If the command has changed, send out the new one
if NewCommand != Command:
dev.ctrl_transfer(0x40, 6, 0x100, 0, NewCommand, 1000)
Command = NewCommand
try:
# Loop forever
while True:
time.sleep(0.1)#time.sleep(0.1)
# read in events
events = pygame.event.get()
# and process them
for event in events:
ProcessArm(event)
except KeyboardInterrupt:
j.quit()#End joystick input
For more information on the project above, go to:
Hiren's time was funded through Nuffield Foundation's Research Placement Scheme. The robot arm is part of Santander UK funding for Robots in Schools.
All opinions in this blog are the Author's and should not in any way be seen as reflecting the views of any organisation the Author has any association with. Twitter @scottturneruon
