# swift = SwiftAPI(filters = {'hwid': 'USB VID:PID=2341:0042'})
swift = SwiftAPI() # default by filters: {'hwid': 'USB VID:PID=2341:0042'}
print('sleep 2 sec ...')
sleep(2)
print('device info: ')
print(swift.get_device_info())
#
print('开始测试')
swift.set_buzzer()
swift.set_buzzer()
print('解锁电机')
swift.set_servo_detach()
sleep(10)
swift.set_buzzer()#TODO 哔哔2声
swift.set_buzzer()
sleep(2)
print('解锁电机')
swift.set_servo_attach()
sleep(3)
print('重置机械臂')
swift.set_buzzer()
swift.reset(x=103,
y=0,
z=42,
speed=800)
def main():
print('setup swift ...')
swift = SwiftAPI()
sleep(2)
print('device info: ')
device_info = swift.get_device_info()
print(device_info)
print('firmware version:')
fw_version = tuple(int(number) for number in device_info[2].split('.'))
print(fw_version)
fw_has_b_angles = (fw_version >= (3, 1, 16))
print('EEPROM Values: ')
print('-> reference_angle_value:')
# in uArmParams.h
# define EEPROM_ON_CHIP 0
# define EEPROM_REFERENCE_VALUE_ADDR 800
# define EEPROM_REFERENCE_VALUE_B_FLAG_ADDR 818
# define EEPROM_REFERENCE_VALUE_B_ADDR 820
# define DATA_TYPE_BYTE 1
# define DATA_TYPE_INTEGER 2
#
# in uArmCalibration.cpp
# uint8_t reference_angle_flag =
# getE2PROMData(EEPROM_ON_CHIP, EEPROM_REFERENCE_VALUE_B_FLAG_ADDR, DATA_TYPE_BYTE);
# reference_angle_value[X_AXIS] =
# getE2PROMData(EEPROM_ON_CHIP, EEPROM_REFERENCE_VALUE_ADDR, DATA_TYPE_INTEGER);
# reference_angle_value[Y_AXIS] =
# getE2PROMData(EEPROM_ON_CHIP, EEPROM_REFERENCE_VALUE_ADDR+2, DATA_TYPE_INTEGER);
# reference_angle_value[Z_AXIS] =
# getE2PROMData(EEPROM_ON_CHIP, EEPROM_REFERENCE_VALUE_ADDR+4, DATA_TYPE_INTEGER);
reference_angle_flag = 0
if fw_has_b_angles:
reference_angle_flag = send_cmd_sync_ok(swift, 'M2211 N0 A818 T1')
reference_angle_value = []
reference_angle_addr = 800
if reference_angle_flag:
reference_angle_addr = 820
for offset in range(0, 6, 2):
reference_angle_value.append(
send_cmd_sync_ok(
swift, 'M2211 N0 A%d T2' % (reference_angle_addr + offset),
SIMPLE_RESPONSE_REGEX))
print(reference_angle_value)
print("-> height offset:")
# in uArmParams.h
# define EEPROM_HEIGHT_ADDR 910
# define DATA_TYPE_FLOAT 4
# in uArmCalibration.cpp
# getE2PROMData(EEPROM_ON_CHIP, EEPROM_HEIGHT_ADDR, DATA_TYPE_FLOAT);
height_offset = send_cmd_sync_ok(swift, "M2211 N0 A910 T4",
SIMPLE_RESPONSE_REGEX)
print(height_offset)
print("-> front offset:")
# in uArmParams.h
# define EEPROM_FRONT_ADDR 920
# in uArmCalibration.cpp
# getE2PROMData(EEPROM_ON_CHIP, EEPROM_FRONT_ADDR, DATA_TYPE_FLOAT);
front_offset = send_cmd_sync_ok(swift, "M2211 N0 A920 T4",
SIMPLE_RESPONSE_REGEX)
print(front_offset)
# print('resetting...')
# swift.reset()
# swift.set_servo_detach(2, wait=True)
# sleep(1)
# swift.set_servo_detach(1, wait=True)
# sleep(1)
# swift.set_servo_detach(0, wait=True)
swift.set_servo_detach(wait=True)
print("Servos detatched")
# print("now you can position the arm on the X-Z plane (rotation locked at zero)")
while True:
sleep(1)
print("Make your selection:")
print(" - Press enter to get calibration data")
print(" - Press s to set reference angle")
if fw_has_b_angles:
print(" - Press b to set reference angle B")
else:
print(" - [reference angle B not supported by firmware %s]" %
fw_version)
print(" - Press h to zero height")
print(" - Press Ctrl-C to exit")
raw_in = input("...")
# swift.set_servo_attach(wait=True)
# print("get_position: %s" % swift.get_position())
# hack to update current position on device
send_cmd_sync_ok(swift, "M2400 S1")
values = OrderedDict()
# Raw sensor values
response = send_cmd_sync_ok(swift, "P2242")
values['sensor'] = OrderedDict([(token[0], token[1:])
for token in response.split(" ")[1:]])
# Cartesian coordinates
response = send_cmd_sync_ok(swift, "P2220")
values['cartesian'] = OrderedDict([
(token[0], token[1:]) for token in response.split(" ")[1:]
])
# angle values
values['angle'] = OrderedDict()
for index, name in [(0, 'B'), (1, 'L'), (2, 'R')]:
response = send_cmd_sync_ok(swift, "P2206 N%s" % index)
values['angle'][name] = response.split(" ")[1][1:]
print("; ".join([
", ".join([
"%s:%+07.2f" % (key, float(value))
for key, value in values[dictionary].items()
]) for dictionary in ['sensor', 'cartesian', 'angle']
]))
if raw_in == "s":
print("M2401: %s" % send_cmd_sync_ok(swift, "M2401 V22765"))
elif fw_has_b_angles and raw_in == "b":
print("M2401: %s" % send_cmd_sync_ok(swift, "M2401 B"))
elif raw_in == "h":
print("M2410: %s" % send_cmd_sync_ok(swift, "M2410"))
else:
logging.warning("did not understand command: %s" % raw_in)
# swift.set_buzzer()
# swift.set_servo_detach(wait=True)
print('done ...')
while True:
sleep(1)
