ps3 wireless controlled MeArm robot part 2

Following on from the first part of this article, I have now managed to add the facility to teach the arm a given sequence of instructions which is remembers, and can subsequently replay. I have also added the facility to save the sequence in a single reusable file, so that it can be reloaded and played on a subsequent run of the program.

I will not describe again the original program, and the new program will require the same supporting items, eg piconzero library and ps3 joystick support as described in the first part of the article.

The new program  listing is shown below.

#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
#version 2 adds facility to record and playback and save robot actions

import subprocess,sys #setup required python libraries
import pygame
import piconzero as pz, time
import cPickle #to save the coms list

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

coms=[]
recordflag=0
playflag=0

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
        brecordstop=ps3.get_button(8) #record_stop
        brecordstart=ps3.get_button(9) #record start
        breplay=ps3.get_button(2) #playback
        breset=ps3.get_button(10) #set servos to start position
        bsavepickle=ps3.get_button(11) #used to save coms list
        bloadpickle=ps3.get_button(0) #used to load coms list
        #successive if statements mean more than one input can be actioned on each pass

        if brecordstart==1: #setup to start recording
            del coms[:] #delete any existing commands saved
            recordflag=1
            print("recording")
            timeoffset=pygame.time.get_ticks() #get starting time
        if brecordstop==1:
            print("record stop")
            recordflag=0
        if (breplay==1) and (len(coms)>0): #only start replay if something recorded ie coms has an entry
            playflag=1
            playcount=0 #set playback starting position
            sval=coms[0][0] #get initial time
        if breset ==1:
            print("Reset") 
            panVal = 90
            lowerVal = 160
            gripVal = 150
            upperVal = 78
        if bsavepickle == 1: #save comslist to a file
            cPickle.dump(coms, open('comsave.p', 'wb'))
        if bloadpickle == 1: #load comslist from file
            coms=cPickle.load(open('comsave.p','rb'))  
        if bleft == 1: #check for pan to the left
            panVal=min(177,panVal + 3)
            if recordflag==1:
                coms.append([pygame.time.get_ticks()-timeoffset,pan,panVal])
        if bright == 1: #check for pan to the right
            panVal=max(0, panVal - 3)
            if recordflag==1:
                coms.append([pygame.time.get_ticks()-timeoffset,pan,panVal])
        if lud < -0.5: #check for left joystick reduce             lowerVal=max(80,lowerVal -2)             if recordflag==1:                 coms.append([pygame.time.get_ticks()-timeoffset,lower,lowerVal])         if lud > 0.5: #check for left joystick increase
            lowerVal = min(180,lowerVal + 2)
            if recordflag==1:
                coms.append([pygame.time.get_ticks()-timeoffset,lower,lowerVal])
        if bopen == 1: #check for grip open
            gripVal=max( 90,gripVal - 5)
            if recordflag==1:
                coms.append([pygame.time.get_ticks()-timeoffset,grip,gripVal])
        if bclose == 1: #check for grip close
            gripVal=min(179,gripVal + 5)
            if recordflag==1:
                coms.append([pygame.time.get_ticks()-timeoffset,grip,gripVal])
        if rud < -0.5: #check for right joystick increase             upperVal=min(162,upperVal+2)             if recordflag==1:                 coms.append([pygame.time.get_ticks()-timeoffset,upper,upperVal])         if rud > 0.5: #check for right joystick reduce
            upperVal=max(56,upperVal-2)
            if recordflag==1:
                coms.append([pygame.time.get_ticks()-timeoffset,upper,upperVal])
        if playflag==0:  #normal use. Execute inputs directly                   
            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

        else: #replay mode. get inputs from coms list
            pygame.time.wait(coms[playcount][0]-sval)
            sval=coms[playcount][0]
            pz.setOutput(coms[playcount][1],coms[playcount][2])
            print ("step: "+str(playcount)+ " output servo channel: "+str(coms[playcount][1])+ " servo setting: "+str(coms[playcount][2]))
            playcount +=1
            if playcount==len(coms):
                playflag=0
              
      
    except KeyboardInterrupt: #continue until ctrl-C
        print
        print ("Exiting")
        #print coms  #uncomment for debugging
        pygame.quit() #clean up pygame and reset picon zero board
        pz.cleanup()
        print ("Cleaned up")
        sys.exit(1) #use to ignore error retrace

Six extra buttons are used on the PS3 controller to facilitate the new features.
Buttons 8 and 9 (the rectangular and arrowhead buttons in the centre of the controller, are used to initiate and stop recording a sequence of instructions. Each time the record button is pressed a new sequence is started. However pressing the right hand joystick button will save the current sequence of instructions to a file, and these can subsequently be reloaded by pressing button 0 (the left hand one of the four on the right hand side). Once a recorded sequence is in place it can be replayed by pressing button 2, the right hand one of the group of four. The final new feature added is to press the left hand joystick button, and this resets the servos to their starting position. This is best done once the position has been approximately located by hand, as it is quite a “fierce” change otherwise. A diagram below shows the various buttons used. (double click for a larger size)

ps3buttons

When the record button is pressed it does the following:

        if brecordstart==1: #setup to start recording
            del coms[:] #delete any existing commands saved
            recordflag=1
            print("recording")
            timeoffset=pygame.time.get_ticks() #get starting time

first it deletes any existing recording held in the list coms. Then it sets a recordflag to 1, and makes a note of the current pygame time in timeoffset.
Subsequent commands initiated by other buttons note the recordflag setting and save their output appending it to the list coms, saving the timestamp, the output channel and the servo value each time they are triggered. e.g. if the grip open button bopen is pressed

        if bopen == 1: #check for grip open
            gripVal=max( 90,gripVal - 5)
            if recordflag==1:
                coms.append([pygame.time.get_ticks()-timeoffset,grip,gripVal])

the additional if statement appends three values to the coms list, the time elapsed in ms from when the record button was pressed, the channel (grip) and the servo setting gripVal.

When the recording process has been finished the recordstop button (8) is pressed, it merely resets the recordflag to 0, preventing any further items being appended to the coms list.

        if brecordstop==1:
            print("record stop")
            recordflag=0

The replay button (2) carries out the commands below:

        if (breplay==1) and (len(coms)>0): #only start replay if something recorded ie coms has an entry
            playflag=1
            playcount=0 #set playback starting position
            sval=coms[0][0] #get initial time

It first checks to see if anything has been recorded (in which case coms will not be empty), and if so sets the playflag to 1
It initialises the playcount to 0, which is used to select the next coms entry in sequence for replay, and retrieves the first offset time from coms[0][0] (The first entry in the first list of three in the coms list. Setting the playflag alters the path through the main loop. If playflag =0 then the path is as in the first program, but when playflag=1 a different option is chosen as shown below.

        if playflag==0:  #normal use. Execute inputs directly                   
            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

        else: #replay mode. get inputs from coms list
            pygame.time.wait(coms[playcount][0]-sval)
            sval=coms[playcount][0]
            pz.setOutput(coms[playcount][1],coms[playcount][2])
            print ("step: "+str(playcount)+ " output servo channel: "+str(coms[playcount][1])+ " servo setting: "+str(coms[playcount][2]))
            playcount +=1
            if playcount==len(coms):
                playflag=0

In this case the else: #replay mode route is followed.
The program waits for the time difference between the time stamp the current and previous commands being replayed, and the stores the timestamp of the current command in sval ready for the next iteration. A call is them made to the piconzero output routine pz.setOutput using the stored values from the current coms list entry set by coms[playcount][1] the servo channel and coms[playcount][2] the servo value. The playcount index is then incremented ready for the next pass. After the last entry has been accessed it sets playflag back to 0, terminating the playback process, by switching the choice back to “normal” in the if playflag==1 statement above.

So that the sequence can be saved and used again on a subsequent run of the program, the bsavepickle button (11) is used to initiate pythons cPickle routing which is used to save the coms list in an internal format to the file comsave.p using the code below.

        if bsavepickle == 1: #save comslist to a file
            cPickle.dump(coms, open('comsave.p', 'wb'))

In the same way the bloadpickle button (0) loads the file back again into the coms list using the code below

        if bloadpickle == 1: #load comslist from file
            coms=cPickle.load(open('comsave.p','rb'))  

Note in neither case do I do any error checking, although this should not be big issue. You will for example get a fatal error if you try and load the coms.p file before it has been written!

These additions make the project much more interesting, and give a good insight into how for example a commercial robot arm might be used in a factory.

A video showing the arm being taught a sequence and subsequently replaying it is here

You can download this second version of the program here

Advertisements