def setPose(self, e=None):
""" Write a pose out to the robot. """
if self.port != None:
if self.curpose != "":
# update pose in project
for servo in range(self.parent.project.count):
self.parent.project.poses[self.curpose][servo] = self.servos[servo].position.GetValue()
print "Setting pose..."
if self.port.hasInterpolation == True: # lets do this smoothly!
# set pose size -- IMPORTANT!
print "Setting pose size at " + str(self.parent.project.count)
self.port.execute(253, 7, [self.parent.project.count])
# download the pose
self.port.execute(253, 8, [0] + project.extract(self.parent.project.poses[self.curpose]))
self.port.execute(253, 9, [0, self.deltaT%256,self.deltaT>>8,255,0,0])
self.port.execute(253, 10, list())
else:
# aww shucks...
#curPose = list() TODO: should we use a syncWrite here?
for servo in range(self.parent.project.count):
pos = self.servos[servo].position.GetValue()
self.port.setReg(servo+1, P_GOAL_POSITION_L, [pos%256, pos>>8])
self.parent.project.poses[self.curpose][servo] = self.servos[servo].position.GetValue()
# pos = self.servos[servo].position.get()
# curPose.append( (servo+1, pos%256, pos>>8) )
#self.pose.syncWrite(P_GOAL_POSITION_L, curPose)
else:
self.parent.sb.SetBackgroundColour('RED')
self.parent.sb.SetStatusText("Please Select a Pose",0)
self.parent.timer.Start(20)
else:
self.parent.sb.SetBackgroundColour('RED')
self.parent.sb.SetStatusText("No Port Open",0)
self.parent.timer.Start(20)
def writePose(self, pose, dt):
# set pose size -- IMPORTANT!
self.port.execute(253, 7, [self.parent.project.count])
# download the pose
self.port.execute(253, 8, [0] + project.extract(pose))
self.port.execute(253, 9, [0, dt % 256, dt >> 8, 255, 0, 0])
self.port.execute(253, 10, list())
def writePose(self, pose, dt):
# set pose size -- IMPORTANT!
self.port.execute(253, 7, [self.parent.project.count])
# download the pose
self.port.execute(253, 8, [0] + project.extract(pose))
self.port.execute(253, 9, [0, dt%256, dt>>8,255,0,0])
self.port.execute(253, 10, list())
def writePose(self, pose, dt):
if not self.port.hasInterpolation:
for servo in range(self.parent.project.count):
value = pose[servo + 1]
self.port.setReg(servo + 1, P_GOAL_POSITION_L,
[value%256, value>>8])
else:
# set pose size -- IMPORTANT!
self.port.execute(253, 7, [self.parent.project.count])
# download the pose
self.port.execute(253, 8, [0] + project.extract(pose))
self.port.execute(253, 9, [0, dt%256, dt>>8,255,0,0])
self.port.execute(253, 10, list())
def setPose(self, e=None):
#self.deltaT = 2000 # TODO: calculate this or add a button
NUM_SERVOS = 5
""" Write a pose out to the robot. """
if self.port != None:
print "Setting pose..."
if self.port.hasInterpolation == True: # lets do this smoothly!
# set pose size -- IMPORTANT!
self.port.execute(253, 7, [NUM_SERVOS])
# load the pose to arbotix
#self.port.execute(253, 8, [0] + project.extract(self.dynamixels))#these two are equivelent
self.port.execute(253, 8, [0] + project.extract(self.parent.project.poses[self.curpose]))
self.port.execute(253, 9, [0, self.deltaT%256,self.deltaT>>8,255,0,0])
self.port.execute(253, 10, list())
else:
self.parent.sb.SetStatusText("Please Select a Pose")
else:
self.parent.sb.SetStatusText("No Port Open")
def runSeq(self, e=None):
""" download poses, seqeunce, and send. """
self.save() # save sequence
if self.port != None:
if self.curseq != "":
print "Run sequence..."
poseDL = dict() # key = pose name, val = index, download them after we build a transition list
tranDL = list() # list of bytes to download
for t in self.parent.project.sequences[self.curseq]:
p = t[0:t.find("|")] # pose name
dt = int(t[t.find("|")+1:]) # delta-T
if p not in poseDL.keys():
poseDL[p] = len(poseDL.keys()) # get ix for pose
# create transition values to download
tranDL.append(poseDL[p]) # ix of pose
tranDL.append(dt%256) # time is an int (16-bytes)
tranDL.append(dt>>8)
tranDL.append(255) # notice to stop
tranDL.append(0) # time is irrelevant on stop
tranDL.append(0)
# set pose size -- IMPORTANT!
print "Setting pose size at " + str(self.parent.project.count)
self.port.execute(253, 7, [self.parent.project.count])
# send poses
for p in poseDL.keys():
print "Sending pose " + str(p) + " to position " + str(poseDL[p])
self.port.execute(253, 8, [poseDL[p]] + project.extract(self.parent.project.poses[p]))
print "Sending sequence: " + str(tranDL)
# send sequence and play
self.port.execute(253, 9, tranDL)
# run or loop?
if e.GetId() == self.BT_LOOP:
self.port.execute(253,11,list())
else:
self.port.execute(253, 10, list())
self.parent.sb.SetStatusText('Playing Sequence: ' + self.curseq)
else:
self.parent.sb.SetBackgroundColour('RED')
self.parent.sb.SetStatusText("Select a Sequence",0)
self.parent.timer.Start(20)
else:
self.parent.sb.SetBackgroundColour('RED')
self.parent.sb.SetStatusText("No Port Open",0)
self.parent.timer.Start(20)
def setPose(self, e=None):
""" Write a pose out to the robot. """
if self.port != None:
if self.curpose != "":
# update pose in project
for servo in range(self.parent.project.count):
self.parent.project.poses[self.curpose][
servo] = self.servos[servo].position.GetValue()
print "Setting pose..."
if self.port.hasInterpolation == True: # lets do this smoothly!
# set pose size -- IMPORTANT!
print "Setting pose size at " + str(
self.parent.project.count)
self.port.execute(253, 7, [self.parent.project.count])
# download the pose
self.port.execute(253, 8, [0] + project.extract(
self.parent.project.poses[self.curpose]))
self.port.execute(
253, 9,
[0, self.deltaT % 256, self.deltaT >> 8, 255, 0, 0])
self.port.execute(253, 10, list())
else:
# aww shucks...
#curPose = list() TODO: should we use a syncWrite here?
for servo in range(self.parent.project.count):
pos = self.servos[servo].position.GetValue()
self.port.setReg(servo + 1, P_GOAL_POSITION_L,
[pos % 256, pos >> 8])
self.parent.project.poses[self.curpose][
servo] = self.servos[servo].position.GetValue()
# pos = self.servos[servo].position.get()
# curPose.append( (servo+1, pos%256, pos>>8) )
#self.pose.syncWrite(P_GOAL_POSITION_L, curPose)
else:
self.parent.sb.SetBackgroundColour('RED')
self.parent.sb.SetStatusText("Please Select a Pose", 0)
self.parent.timer.Start(20)
else:
self.parent.sb.SetBackgroundColour('RED')
self.parent.sb.SetStatusText("No Port Open", 0)
self.parent.timer.Start(20)
def setPose(self, e=None):
#self.deltaT = 2000 # TODO: calculate this or add a button
NUM_SERVOS = 5
""" Write a pose out to the robot. """
if self.port != None:
print "Setting pose..."
if self.port.hasInterpolation == True: # lets do this smoothly!
# set pose size -- IMPORTANT!
self.port.execute(253, 7, [NUM_SERVOS])
# load the pose to arbotix
#self.port.execute(253, 8, [0] + project.extract(self.dynamixels))#these two are equivelent
self.port.execute(
253, 8, [0] +
project.extract(self.parent.project.poses[self.curpose]))
self.port.execute(
253, 9,
[0, self.deltaT % 256, self.deltaT >> 8, 255, 0, 0])
self.port.execute(253, 10, list())
else:
self.parent.sb.SetStatusText("Please Select a Pose")
else:
self.parent.sb.SetStatusText("No Port Open")
