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

This is the third and final part of my project using a MeArm robot driven by a 4Tronix picon zero board from a Raspberry Pi. Previously I have developed a program to control the arm with a wireless Ps3 controller (see part 1), and in part 2 I added the facility to record and replay a sequence of moves using the arm. In this third pard I add three further sets of commands to the repertoire, and also add some checks to ensure the safe operation of the arm.

First I have added a button to check if there is a recorded sequence of commands recorded in memory. This uses the centre button on the controller and either displays “No recorded sequence” or “Recorded sequence in memory” depending upon which applies.

The second pair of new commands enables you to pause or continue a playback sequence. This is achieved by using the “hat” control on the ps3. Pressing the down arrow stops playback, and pressing the up arrow continues the playback. If you press one of the control keys eg l=pan left, open grip etc whilst the playback is paused, then you will be unable to restart the playback sequence as the program realises that the arm has moved away from its position in the. playback sequence.

The third new command is to add a second playback mode to the program. Previously the arm replayed commands in real time. That is to say the timings were exactly the same as when they were recordewd. A second fastplayback mode replaces all the timing with a small fixed gap between each command. This can give a smoother and slightly quicker playback, removing any long puases whichh were in the recorded sequence. The original playback sequence used the right hand button of the group of four on the top of the ps3 controller, The new fastplayback uses the top buton of gthese four. (See diagram later on)

It is important that the arm starts from a know position when starting recording or playback. I have altered the program in the following ways. First the action of the reset button is now added to the recording sequence when in record mode. It is suggested that this is recorded as the last command in a record sequence. This will mean that the sequence can be replayed immediately after recording, because otherwise, I have prevented the playback buttons, from activating unless the arm is in the reset position. Likewise the record button will only activate and print recording on the the screen if the arm is in the reset position before the recording starts.

The program is now quite long, and several extra flags have been added to facilitate the logic of the new commands. There are comments in the program, and I do not intend to go through it again in detail, as in the first two parts of the series. IT is listed below, and there is a download link at the end of the article. Also at the end of the article is a link to a video which details the operation of the arm, and then shows it replaying a complex sequence of operations.

#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 robo actions
#version 3 adds facility to pause and restart playback, also gfastplayback mode
#In addition failsafe locks are added to ensure recording and playback start from zeroed position
#Finally a command is added to signal whether there is a recorded sequence stored in memory ready for playback

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=[] #used to store recorded sequence
recordflag=0 #used to signify recording in progress
playflag=0 #used to signify playback in progress
interrupted=0 #used to signify playback has been paused when 1. NB set to 0 by any manual command which alters servos
fastPlayback=0 #used to signify fastplayback has been selected

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 NB these also have to be set at start of playback and fastplayback commands
panVal = 90
lowerVal = 160
gripVal = 150
upperVal = 78

def valUpdate (output,val): #used to update Val variables and output next recorded state to servos
    global panVal,lowerVal,upperVal,gripVal
    if output==0:
        panVal=val
    if output==1:
        lowerVal=val
    if output==2:
        upperVal=val
    if output==3:
       gripVal=val
    pz.setOutput(output,val)  


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
        breplayfast=ps3.get_button(3) #playback in fast mode
        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
        hatstate=ps3.get_hat(0) #used to pause/restart playback
        bquerycoms=ps3.get_button(12) #used to check if there is a recorded sequence in memory

        #successive if statements mean more than one input can be actioned on each pass

        #check in reset position before allowing record. Check "recording" displayed on screen
        if brecordstart==1 and panVal ==90 and lowerVal == 160 and gripVal == 150 and upperVal ==78: #setup to start recording
            del coms[:] #delete any existing commands saved
            recordflag=1
            print("recording") #disappears fast from screen but useful for debugging if button held
            pygame.time.wait(200)           
            timeoffset=pygame.time.get_ticks() #get starting time
        if brecordstop==1:
            print("record stop")
            recordflag=0 #reset recordflag
        #check arm is in reset position before allowing playback
        if (breplay==1) and (len(coms)>0) and panVal ==90 and lowerVal == 160 and gripVal == 150 and upperVal ==78: #only start replay if something recorded ie coms has an entry
            fastplayBack=0 #set normal playback mode
            playflag=1 #set playback flag
            interrupted=0 #set interrupt flag to 0 (not interrupted)
            playcount=0 #set playback starting position
            sval=coms[0][0] #get initial time
        #check arm is in reset position before allowing fastplayback
        if (breplayfast==1) and (len(coms)>0) and panVal ==90 and lowerVal == 160 and gripVal == 150 and upperVal ==78: #only start replay if something recorded ie coms has an entry
            fastPlayback=1 #set fastplayback mode
            playflag=1 #set playback flag
            interrupted=0 #set interrupt flag to 0 (not interrupted)
            playcount=0 #set playback starting position
            sval=coms[0][0] #get initial time
        if hatstate[1]==-1 and playflag==1:
            playflag=0 #stop playback
            interrupted = 1 #set interrupted state flag
        if hatstate[1]==1 and interrupted==1: #if interrupted then restore playback
           playflag=1 #set playback flag again
           interrupted=0 #reset interrupted state
        if breset ==1:
            interrupted=0 #reset interrupted for all commands that alter servo positions
            print("Reset") 
            panVal = 90
            if recordflag==1: #then record reset commands
                coms.append([pygame.time.get_ticks()-timeoffset,pan,panVal])
            lowerVal = 160
            if recordflag==1:
                coms.append([pygame.time.get_ticks()-timeoffset,lower,lowerVal])
            gripVal = 150
            if recordflag==1:
                coms.append([pygame.time.get_ticks()-timeoffset,grip,gripVal])
            upperVal = 78
            if recordflag==1:
                coms.append([pygame.time.get_ticks()-timeoffset,upper,upperVal])
        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 bquerycoms==1: #check if recorded program in coms. Hold button down to see this
            if len(coms)>0:
                print "Recorded sequence in memory"
            else:
                print "No recorded sequence"               
        if bleft == 1: #check for pan to the left
            interrupted=0
            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
            interrupted=0
            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             interrupted=0             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
            interrupted=0
            lowerVal = min(180,lowerVal + 2)
            if recordflag==1:
                coms.append([pygame.time.get_ticks()-timeoffset,lower,lowerVal])
        if bopen == 1: #check for grip open
            interrupted=0
            gripVal=max( 90,gripVal - 5)
            if recordflag==1:
                coms.append([pygame.time.get_ticks()-timeoffset,grip,gripVal])
        if bclose == 1: #check for grip close
            interrupted=0
            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             interrupted=0             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
            interrupted=0
            upperVal=max(56,upperVal-2)
            if recordflag==1:
                coms.append([pygame.time.get_ticks()-timeoffset,upper,upperVal])

        #now update the servos, either in normal and record mode, or in replay mode
        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
            if fastPlayback==1:
                pygame.time.wait(50) #playback with constant 50ms between updates
            else:
                pygame.time.wait(coms[playcount][0]-sval) #playback with recorded times between updates
            sval=coms[playcount][0]
            valUpdate(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
                fastPlayback=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

updated ps3 controller button allocations (double click for large view)

updatedps3

You can see a video with full details here

You can download the program here

Advertisements