PS3 wireless controlled MeArm using Pi and 4Tronix picon zero board


IMG_4552

Although my main interest in Raspberry Pi is in using Sonic Pi, I do like to delve from time to time into the world of robotics and control. A year or two ago I built the Maplin robot arm, and more recently I modified this to be controlled by a PiZero plus a motor control board. The project used buttons on a keyboard to control the motors and was rather unwieldy and difficult to control.

More recently I acquired a MeArm robot kit. This is a lightweight robot arm controlled by four servos. Although the deluxe kit is supplied with a controller board and a joystick controller board, I wanted to see if I could control it from a Raspberry Pi. However, if possible I also wanted to avoid using keys on a keyboard or on a graphical screen to control the robot, preferring to explore If I could control it with a games controller. I had burned my fingers before on this, buying a cheap wii-remote which I could never get to pair with my Pi and be recognised by it. At the recent Potton Pi & Pint meeting, I chatted with Brian Corteil who recommended the Ps3 AfterGlow controller, which I managed to pick up for £16.89 from gadget.exchange on the Amazon Store.

IMG_4553

This controller requires very little in the way of installation, coming with its own wireless dongle which you plug into the Pi. I got details of setting it up and testing from an article on the Piborg Site at https://www.piborg.org/joyborg All it needs is:

sudo apt-get -y install joystick

and you can then test it with

 jstest /dev/input/js0

The rest of the program in that article gives an example of motor control using a piborg robot.

Recently I spotted the 4tronix picon zero board. This seemed very versatile, (see a review here http://www.recantha.co.uk/blog/?p=14715) ,especially as amongst other outputs it could drive up to 6 servo motors, and I decided to purchase one.

IMG_4551

The board is supported with a series of worksheets, together with a test suite of programs. These (together with necessary library) are easily downloaded from the 4tronix site, from a page on their blog.

http://4tronix.co.uk/blog/?p=1224

Having installed the library, I tried out the program servoTest.py, using a couple of spare servers connected to two of the 6 servo outputs. This worked as expected, so I then started looking at replacing the keyboard inputs with input from the ps3 controller. I googled quite a bit and found a variety of examples for setting up a controller, but the shortest that I came across was at http://archives.seul.org/pygame/users/Nov-2012/msg00051.html and consisted of the code below, modified a bit as described:

import subprocess
import pygame

pygame.init()
clock = pygame.time.Clock() #added line
ps3 = pygame.joystick.Joystick(0)
ps3.init()

while True:
    try:
        pygame.event.pump()
        #removed refs to buttons 13-16 in the following line
        buttons = [ps3.get_button(0),ps3.get_button(1),ps3.get_button(2),ps3.get_button(3),ps3.get_button(4),ps3.get_button(5),ps3.get_button(6),ps3.get_button(7),ps3.get_button(8),ps3.get_button(9),ps3.get_button(10),ps3.get_button(11),ps3.get_button(12)]
        axis = [ps3.get_axis(0),ps3.get_axis(1),ps3.get_axis(2),ps3.get_axis(3)]
        hat=[ps3.get_hat(0) #added line
        print(buttons)
        print(axis)
        print (hat)  #added line
        clock.tick(20)  #added line
        subprocess.call("clear")
    except KeyboardInterrupt:
        print("Exiting")
        raise

I removed references to buttons 13..16 as my controller only had a total of 13 buttons and the code worked well. I also added 4 lines indicated in the listing. Two added a pause in the loop, as it tended to flicker {clock = pygame.time(Clock) and clock.tick(20)} and the other two got data about the “hat” control on the controller {hat = [ ps3.get_hat(0)] and print (hat)}, although I didn’t make use of the hat in the project.
NB you have to run from the desktop graphics environment, even though the code doesn’t set up a separate window, or you will get an error.
There were more sophisticated programs such as http://pygame.org/wiki/Joystick_analyzer (requires python3 to run) if you want a “nicer” output, but I wanted to keep things as simple as possible.
After that it was a matter of experimenting and seeing how to fit the pieces together. The picon zero examples were written for python 2.7 so I decided to stick with that. Initially I used the commands to read values from the joystick axes on the controller such as ps3.get_axis(0) and ps3.get_axis(1) to read the values from the joysticks. I used a mapping function to translate the -1 to +1 range into a suitable range for the servo motor input (0 to 180) and I successfully got the arm working with these. (NB some ranges were reduced as the entire movement was not available on the MeArm for all four servos). However I found that this direct one to one mapping of the two joysticks to the 4 servos made it very difficult to control the arm in practice. The response was fantastic, but it was difficult for example to keep the grip closed whilst trying to lift the arm. Instead I resolved to use the method utilised in the servoTest.py program. This set each servo to a know starting position, and then used keyboard controls to increase or decrease the servo setting within the max and minimum limits allowable. This was done by utilising commands similar to:

if keyp == 'w':
    panVal = min(180,panVal + 5)
elif keyp == 'z'
    panVal = max(0, panVal - 5)

If key w is pressed then the current value of the pan position (panVal) is increased by a step of 5 until it reaches a max value of 180. Conversely if the z key is pressed the pan position decreases by a step of 5 until it reaches a minimum of 0. The clever use of the max and min functions sets the limits of travel. By replacing the detection of the w and z keys by suitable inputs from the ps3 controller the same effect should be achievable.

Here is the final program I came up with.

#Python2.7 program to drive MeArm robot, written by Robin Newman, May 2016
#Arm is connected to a picon zero board, which drives the four servos
#Control is by means of a PS3 wireless AFterGlow controller

import subprocess,sys #setup required phton libraries
import pygame
import piconzero as pz, time

pygame.init() #initialise items
clock = pygame.time.Clock()
ps3 = pygame.joystick.Joystick(0)
ps3.init()
# Define which pins are the servos
pan = 0
lower = 1
grip = 3
upper = 2

pz.init() #initialise the piconzero board

# Set output mode to Servo
pz.setOutputConfig(pan, 2)
pz.setOutputConfig(lower, 2)
pz.setOutputConfig(grip, 2)
pz.setOutputConfig(upper, 2)


# Initialise all servo positions
panVal = 90
lowerVal = 160
gripVal = 150
upperVal = 78
pz.setOutput (pan, panVal)
pz.setOutput (lower, lowerVal)
pz.setOutput (grip, gripVal)
pz.setOutput (upper, upperVal)

while True: #main loop runs until ctrl-c detected
    try:
        pygame.event.pump() #make sure event queue is current
        llr=ps3.get_axis(0) #read the four axes (in fact don't use llr values or rlr in this version)
        lud=ps3.get_axis(1) #used for lower arm
        rlr=ps3.get_axis(2)
        rud=ps3.get_axis(3) #used for upper arm
        bopen=ps3.get_button(6) #open grip
        bclose=ps3.get_button(7)   #close grip
        bleft=ps3.get_button(4) #pan left
        bright=ps3.get_button(5) #pan right
        #panVal=translate(llr,-1,1,180,0)
        #successive if statements mean more than one input can be actioned on each pass
        if bleft == 1: #check for pan to the left
            panVal=min(177,panVal + 3)
        if bright == 1: #check for pan to the right
            panVal=max(0, panVal - 3)
        if lud < -0.5: #check for left joystick reduce lowerVal=max(80,lowerVal -2) if lud > 0.5: #check for left joystick increase
            lowerVal = min(180,lowerVal + 2)
        if bopen == 1: #check for grip open
            gripVal=max( 90,gripVal - 5)
        if bclose == 1: #check for grip close
            gripVal=min(179,gripVal + 5)
        if rud < -0.5: #check for right joystick increase upperVal=min(162,upperVal+2) if rud > 0.5: #check for right joystick reduce
            upperVal=max(56,upperVal-2)
        pz.setOutput (pan, panVal) #now send updated signals to the 4 servos
        pz.setOutput (lower, lowerVal)
        pz.setOutput (grip, gripVal)
        pz.setOutput (upper, upperVal)
        print "panVal "+str(panVal) #now print updated values in the terminal
        print "gripVal "+str(gripVal)
        print "lowerVal "+str(lowerVal)
        print "upperVal "+str(upperVal)
        clock.tick(20) #wait for a bit
        subprocess.call("clear") #clear the screen
    except KeyboardInterrupt: #continue until ctrl-C
        print
        print ("Exiting")
        pygame.quit() #clean up pygame and reset picon zero board
        pz.cleanup()
        print ("Cleaned up")
        sys.exit(1) #use to ignore error retrace

 

IMG_4554

IMG_4555

A couple of words of warning. First the initialised servo position may be slightly different for you if you build this model. The same is true for the range of each servo which is set in the max and min functions for each one. In particular be careful with the upper and lower arm servos. These two servos can cause the arms to try and foul each other. If you adjust their limits to be completely safe, then you will restrict the range of movements of the arm. If you allow them to overlap slightly which I have done her, then it is up to you when driving the arm to make sure that you are aware of this and are careful not to cause problems. Also, you are strongly advised to park the arm near to the starting position, as otherwise there can be severe movement s of the arm when you start the program. You may want to screw the base onto a pice of wood, to prevent the arm toppling over when lifting loads. In practice I mostly got away with weighting the back of the arm base down with the Raspberry Pi.

As my first attempt into remote control with a wireless controller I am pleased with the results. What I hope to develop now is the ability for the arm program to record its inputs for subsequent replay. This will not be easy, but I hope that I can make progress in this area, and I will add to this article if that is the case.

You can see a video of the MeArm in action here

You can download the final program here

Advertisements