def sync_send_write_instruction(self, servos, angs, angvels):
''' move servos to angles (radians)
'''
# verify validity of input commands
for i in range(len(servos)):
if angvels[i] > servos[i].settings['max_speed']:
print 'lib_robotis.move_angle: angvel too high - %.2f deg/s' % (math.degrees(angvel))
print 'lib_robotis.ignoring move command.'
return
if angs[i] > servos[i].settings['max_ang'] or angs[i] < servos[i].settings['min_ang']:
print 'lib_robotis.move_angle: angle out of range- ', math.degrees(angs[i])
print 'lib_robotis.ignoring move command.'
return
msg = [ 0XFF, 0XFF, 0XFE, (4+1) * len(servos) + 4, 0x83, 0X1E, 0X04 ] # header for sync_write message
for i in range(len(servos)):
enc_tics = int(round( angs[i] / servos[i].settings['rad_per_enc'] ))
enc_tics += servos[i].settings['home_encoder']
enc_tics = min( max( enc_tics, 0 ), servos[i].settings['max_encoder'] )
hibyte_ang,lowbyte_ang = enc_tics / 256, enc_tics % 256 # hibyte, lowbyte for angle specification
rpm = angvels[i] / (2 * math.pi) * 60.0
angvel_enc = int(round( rpm / (114.0 / 1024.0) ))
if angvel_enc<0:
hibyte_vel,lowbyte_vel = abs(angvel_enc) / 256 + 4, abs(angvel_enc) % 256
else:
hibyte_vel,lowbyte_vel = angvel_enc / 256, angvel_enc % 256
msg = msg + [ servos[i].servo_id,lowbyte_ang,hibyte_ang,lowbyte_vel,hibyte_vel ]
chksum = self.calc_checksum( msg )
msg = msg + [chksum] # add appropriate checksum value
out = ''
for m in msg:
out += chr(m)
print msg
self.acq_mutex()
try:
self.send_serial( out )
except:
self.rel_mutex()
raise
self.rel_mutex()Statistics: Posted by wolf — Thu Jul 19, 2012 11:32 pm