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)
You can see a video with full details here
You can download the program here