def test_angle2int():
for angle in [0, 90, 180, 270, 300]:
a = int(angle/300.0*1023.0)
assert angle2int(angle, True) == le(a)
for angle in [0, pi/4, pi/2, 3*pi/2]:
a = int(180*angle/300.0/pi*1023.0)
assert angle2int(angle, False) == le(a)
def makeSyncMovePacket(self, info, degrees=True):
"""
Write sync angle information to servos.
info = [[ID, angle], [ID, angle], ...]
ID: servo ID
angle: 0-300 degrees or in radians
"""
data = []
# since all servo angles have the same register addr (GOAL_POSITION)
# and data size (2), a sinc packet is smart choice
# compare bulk vs sync for the same commands:
# bulk = 94 bytes
# sync = 50 bytes
for cmd in info:
dataN = [] # data for specific actuator
dataN.append(cmd[0]) # ID
angle = angle2int(cmd[1], degrees)
dataN.append(angle[0]) # LSB
dataN.append(angle[1]) # MSB
data.append(dataN)
pkt = self.makeSyncWritePacket(self.GOAL_POSITION, data)
return pkt
def main():
port = "/dev/tty.usbserial-AL034G1T"
serial = ServoSerial(port=port)
serial.open()
servo = Packet(pyservos.AX12)
# servo = Packet(pyservos.XL320)
# build
# 59 rpm max
data = []
ids = [1, 2, 3]
pos = [150, 200, 90]
vel = [200, 100, 300] # 0.111 rpm
for i, p, v in zip(ids, pos, vel):
data.append([i] + angle2int(p) + le(v))
pkt = servo.makeSyncWritePacket(pyservos.AX12.GOAL_POSITION, data)
print(pkt)
ans = serial.sendPkt(pkt) # send packet to servo
ans = servo.processStatusPacket(ans)
if ans:
print('status: {}'.format(ans))
def makeServoMovePacket(self, ID, angle, degrees=True):
"""
Commands the servo to an angle (0-300 deg) or radians (0-5.24 rads)
angle: [0-300] deg or [0.0-5.24] rads
degrees: True (default) or False
"""
val = angle2int(angle, degrees)
pkt = self.makeWritePacket(ID, self.GOAL_POSITION, val)
return pkt
def makeServoMovePacket(self, ID, angle, degrees=True):
"""
Commands the servo to an angle (in degrees)
"""
# if degrees and not (0.0 <= angle <= 300.0):
# raise Exception('makeServoMovePacket(), angle [deg] out of bounds: {}'.format(angle))
# elif (not degrees) and (not (0.0 <= angle <= 5.23598776)):
# raise Exception('makeServoMovePacket(), angle [rads] out of bounds: {}'.format(angle))
# val = int(angle/300*1023)
val = angle2int(angle, degrees)
pkt = self.makeWritePacket(ID, self.GOAL_POSITION, val)
return pkt
def makeSyncMoveSpeedPacket(self, info, degrees=True):
"""
Write sync angle information to servos.
info = [[ID, angle, speed], [ID, angle, speed], ...]
ID: servo ID
angle: 0-300 degrees or in radians
speed: 0-1023
"""
data = []
for cmd in info:
dataN = [] # data for specific actuator
dataN.append(cmd[0]) # ID
angle = angle2int(cmd[1], degrees)
dataN.append(angle[0]) # LSB
dataN.append(angle[1]) # MSB
speed = le(cmd[2])
dataN.append(speed[0]) # LSB
dataN.append(speed[1]) # MSB
data.append(dataN)
pkt = self.makeSyncWritePacket(self.GOAL_POSITION, data)
return pkt
def set_angle(id, angle):
val = angle2int(angle, degrees=True) + le(speed)
pkt = servo.makeWritePacket(id, servo.base.GOAL_POSITION, val)
serial.sendPkt(pkt) # send packet to servo