PS3 Controller to move a USB Robot Arm

Guest Blogger Hiren Mistry, Nuffield Research Placement Student working at the University of Northampton.

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:

https://www.pygame.org/docs/ref/joystick.html

https://theraspberryblonde.wordpress.com/2016/06/29/ps3-joystick-control-with-pygame/

Acknowledgements
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