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