def test_ax12_angle_packet():
ax = AX12()
dpkt = ax.makeServoMovePacket(1, 116, degrees=True)
# print(dpkt)
rpkt = ax.makeServoMovePacket(1, 116*pi/180, degrees=False)
# print(rpkt)
# packet_check(dpkt, rpkt)
assert dpkt == rpkt, f"{dpkt} != {rpkt}"
ans = [255, 255, 1, 5, 3, 30, 255, 1, 216]
pkt = ax.makeServoMovePacket(1, 150)
assert ans == pkt, f"{ans} != {pkt}"
def ping(serial, id):
"""
Sends a ping packet to ID's from 0 to maximum and prints out any returned
messages.
Actually send a broadcast and will retry (resend) the ping 3 times ...
"""
# port = "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A904MISU-if00-port0"
retry = 3
valid_return = False
# s = ServoSerial(port) # FIXME
s = serial
s.open()
# try:
# s.open()
# except Exception as e:
# print('-'*40)
# print(sys.argv[0], ':')
# print(e)
# exit(1)
servo = AX12()
pkt = servo.makePingPacket(id)
s.write(pkt)
time.sleep(0.1)
found_servos = {}
# as more servos add up, I might need to increase the cnt number???
for cnt in range(retry):
ans = s.read()
if ans:
valid_return = True
pkts = servo.decodePacket(ans)
for pkt in pkts:
info = servo.processStatusPacket(pkt)
if info['id'] not in found_servos.keys():
found_servos[info['id']] = info
time.sleep(0.1)
if valid_return:
keys = list(found_servos.keys())
keys.sort()
for key in keys:
print_status_pkt(found_servos[key])
else:
print('No servos found')
def test_ax12_fail_find_packets():
ax = AX12()
data = [
0xff, 0xff, # random header
0xff, 0xff, 0x01, 0x04, 0x02, 0x2b, 0x01, 0x11 # bad packet, crc is wrong
]
with pytest.raises(Exception):
ans = ax.find_packets(data)
assert len(ans) == 1
for a in ans:
assert a
def test_ax12_check_sum():
# data packets from:
# http://support.robotis.com/en/product/actuator/dynamixel/communication/dxl_instruction.htm
data = [
[0xff, 0xff, 0x01, 0x04, 0x02, 0x2b, 0x01, 0xcc], # read data
[0xff, 0xff, 0xfe, 0x04, 0x03, 0x03, 0x1, 0xf6], # write data
[0xFF, 0xFF, 0x01, 0x02, 0x24, 0xD8], # overload and over heat error
[0xff, 0xff, 0x01, 0x02, 0x01, 0xfb], # reg write
[0xff, 0xff, 0x00, 0x02, 0x06, 0xf7], # reset
[0xff, 0xff, 0x01, 0x04, 0x02, 0x00, 0x03, 0xf5], # read firmware
[0xff, 0xff, 0x01, 0x02, 0x00, 0xfc], # led status packet, no error
[0xff, 0xff, 0xfe, 0x18, 0x83, 0x1e, 0x04, 0x00, 0x10, 0x00, 0x50, 0x01, 0x01, 0x20, 0x02, 0x60, 0x03, 0x02, 0x30, 0x00, 0x70, 0x01, 0x03, 0x20, 0x02, 0x80, 0x03, 0x12] # sync write
]
for d in data:
cs = AX12.check_sum(d[2:-1])
assert cs == d[-1]
def test_ax12_find_packets():
ax = AX12()
# 2 good packets
data = [
0x3, 0x4, # noise
0xff, 0xff, 0x01, 0x04, 0x02, 0x2b, 0x01, 0xcc,
0xff, 0xff, # random header
0xff, 0xff, 0xfe, 0x18, 0x83, 0x1e, 0x04, 0x00, 0x10, 0x00, 0x50, 0x01, 0x01, 0x20, 0x02, 0x60, 0x03, 0x02, 0x30, 0x00, 0x70, 0x01, 0x03, 0x20, 0x02, 0x80, 0x03, 0x12 # sync write
]
ans = ax.find_packets(data)
assert len(ans) == 2
for a in ans:
assert a
def test_ax12_sync_write():
ax = AX12()
path = [
# ID position velocity
[0, 0x10, 0x00, 0x50, 0x01],
[1, 0x20, 0x02, 0x60, 0x03],
[2, 0x30, 0x00, 0x70, 0x01],
[3, 0x20, 0x02, 0x80, 0x03]
]
ans = [0xff, 0xff, 0xfe, 0x18, 0x83, 0x1e, 0x4, 0x0, 0x10, 0x0, 0x50, 0x1, 0x1, 0x20, 0x2, 0x60, 0x3, 0x2, 0x30, 0x0, 0x70, 0x1, 0x3, 0x20, 0x2, 0x80, 0x3, 0x12]
pkt = ax.makeSyncWritePacket(AX12.GOAL_POSITION, path)
# for p, a in zip(pkt, ans):
# assert p == a
# packet_check(pkt, ans)
assert pkt == ans, f"{ans} != {pkt}"
# for d in data:
# print(int(d), end=',')
# print(" ")
d = list(bytearray(data))
dd = servo.decodePacket(d)
for d in dd:
print(servo.status_packet(d))
else:
print(f"{Fore.RED}Oops ... {Fore.RESET}")
port = "/dev/serial/by-id/usb-Adafruit_Trinket_M0_F42D3DEC504C5430372E314AFF090732-if00"
ser = Serial(port, 115200)
servo = AX12()
val = 1
while True:
val = 0 if val > 0 else 1
# pkt = servo.makeWritePacket(1, 25, [val])
pkt = servo.makeLEDPacket(1, val)
send(ser, pkt)
sleep(1)
# angles = [0, 150, 300, 150]
# rd = servo.makeReadAnglePacket(1)
# send(ser, rd)
# get(ser, servo)
#
# for a in angles:
def main():
servo = AX12()
args = handleArgs()
# check for common errors
if 'level' in args:
if args['level'] not in [1,2,3]:
print(f"Invalid level: {args['level']}")
# print(args)
print(Fore.RED + str(args) + Fore.RESET)
exit(1)
if 'id' in args:
if not (1 <= args['id'] <= 254):
print(f"Invalid ID: {args['id']}")
# print(args)
print(Fore.RED + str(args) + Fore.RESET)
exit(1)
if args['rate'] not in [9600, 19200, 57600, 115200, 1000000]:
print(f"{Fore.RED}*** {args['rate']} is an invalide buad rate ***{Fore.RESET}")
exit(1)
if args['debug']:
print(Fore.YELLOW + str(args) + Fore.RESET)
# create a serial port
if args['dtr']:
from pyservos.pi_servo_serial import PiServoSerial
serial = PiServoSerial(args['port'], args['dtr'])
else:
serial = ServoSerial(args['port'], baud_rate=args['rate'])
# try to open serial port
try:
serial.open()
except:
print(Back.RED + "-------------------------")
print(f" Invalid port: {args['port']}")
print("-------------------------" + Back.RESET)
exit(1)
# Print serial open success!!!
uname = platform.uname()
print(Back.GREEN + "="*80)
# print("| Servo AX-12A Tool")
print(f"| {uname.node}:{uname.system}")
print(f"| Python {platform.python_version()}")
print(f"| baudrate: {args['rate']}")
print(f"| serial port: {args['port']}")
print("-"*80 + Back.RESET)
# servo = AX12()
choice = args['which']
if choice == "ping":
print(f">> ping {args['id']}")
ping(serial, args['id'])
elif choice == "set_angle":
if 'angle' not in args or 'id' not in args:
raise Exception("Invalid input")
angle = args['angle']
if args['radians']:
angle = angle*180/pi
if not (0 <= angle <= 300):
raise Exception(f"Invalid angle: {angle}")
print(f">> set servo[{args['id']}] angle: {angle}")
pkt = servo.makeServoMovePacket(args['id'], angle)
serial.sendPkt(pkt)
elif choice == "set_id":
if 'current_id' not in args or 'new_id' not in args:
print("Invalid input")
exit(1)
if 0 > args['new_id'] > 253:
print("Invalid ID number, must be between 0-253")
exit(1)
print(f">> set servo[{args['current_id']}] to new ID: {args['new_id']}")
pkt = servo.makeSetIDPacket(args['current_id'], args['new_id'])
serial.write(pkt)
elif choice == "set_baudrate":
pkt = servo.makeBaudRatePacket(args['id'], args['baudrate'])
print(pkt)
ret = serial.sendPkt(pkt)
ans = servo.find_packets(ret)
if ans:
err = "{Fore.GREEN}OK" if ans[4] == 0 else "{Fore.RED}ERROR"
print(f">> Servo {ans[2]} is {err}{Fore.RESET}")
else:
print(f"{Fore.RED}*** Something went wrong, no status packet ***{Fore.RESET}")
elif choice == "get_angle":
# print(Fore.RED + "Not currently implemented" + Fore.RESET)
print(f">> get current angle from servo: {args['id']}")
pkt = servo.makeReadAnglePacket(args['id'])
# pkt = servo.makeServoInfoPacket(args['id'])
# d = [
# # [data len, ID, addr]
# [2, 1, servo.PRESENT_POSITION],
# # [2, 2, servo.PRESENT_POSITION],
# # [2, 3, servo.PRESENT_POSITION],
# ]
# pkt = servo.makeBulkReadPacket(d)
# print(pkt)
ans = serial.sendPkt(pkt)
# print(Fore.BLUE + str(ans) + Fore.RESET)
# print("<<", ans)
ans = servo.find_packets(ans)
if ans:
# print(ans[0])
ans = ans[0]
err = ans[4]
angle = (ans[6]<<8) + ans[5]
deg = angle * 300/1023
print(f">> Angle: {Fore.GREEN}{angle} counts {Fore.CYAN}{deg:.1f} deg{Fore.RESET}")
elif choice == "reboot":
print(f">> reboot servo: {args['id']}")
pkt = servo.makeRebootPacket(args['id'])
serial.write(pkt)
elif choice == "reset":
print(f">> reset servo: {args['id']} to level: {args['level']}")
if args['level'] not in [1,2,3]:
print(Fore.RED + "Invalid input" + Fore.RESET)
exit(1)
pkt = servo.makeResetPacket(args['id'], args['level'])
serial.write(pkt)
# elif args['loop']:
elif choice == "loop":
loop(serial, servo)
serial.close()
def test_ax12_read_angle():
ans = [255, 255, 1, 4, 2, 36, 2, 210]
ax = AX12()
pkt = ax.makeReadAnglePacket(1)
assert pkt == ans, f"{pkt} != {ans}"
def test_ax12_led_fail():
ax = AX12()
with pytest.raises(Exception):
ax.makeLEDPacket(2, 5)
def test_ax12_ping_packet():
ans = [255, 255, 254, 2, 1, 254]
ax = AX12()
pkt = ax.makePingPacket()
assert pkt == ans, f"{pkt} != {ans}"
def test_ax12_bulk_read():
ax = AX12()
ans = [255, 255, 254, 11, 146, 2, 1, 36, 2, 2, 36, 2, 3, 36, 236]
assert True