Showing posts with label ps3. Show all posts
Showing posts with label ps3. Show all posts

Tuesday 8 August 2017

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.




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:


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

ChatGPT, Data Scientist - fitting it a bit

This is a second post about using ChatGPT to do some data analysis. In the first looked at using it to some basic statistics  https://robots...