Instantly share code, notes, and snippets.

Embed
What would you like to do?
MeArmPs3RobotVersion2.py version 2 adds the facility to record and playback movement sequences. Article at https://rbnrpi.wordpress.com/project-list/ps3-wireless-controlled-mearm-robot-part-2/ Video at https://youtu.be/spONiLz-I7M
#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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment