def SetGoalPosition(self, Id, goalPose):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.GOAL_POSITION, goalPose)
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.GOAL_POSITION, goalPose)
if dynamixel.getLastTxRxResult(self.port_num, Registers.PROTOCOL_VERSION) != Registers.COMM_SUCCESS:
#dynamixel.printTxRxResult(Registers.PROTOCOL_VERSION, dynamixel.getLastTxRxResult(self.port_num, Registers.PROTOCOL_VERSION))
return False
elif dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
else:
return True
except :
print "Oops! Problem writing..."
return False
def get_temp(self, num_tries=RETRIES):
retry = 0
while (retry < num_tries):
temp = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_PRESENT_TEMP)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
return temp
raise Exception("get_temp failed after {} tries".format(num_tries))
def get_position(self, num_tries=RETRIES):
retry = 0
while (retry < num_tries):
pos = dxl.read2ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_PRESENT_POSITION)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
return self.value2rad(pos)
raise Exception("get_position failed after {} tries".format(num_tries))
def set_position(self, pos, num_tries = RETRIES):
value = self.rad2value(pos)
retry = 0
while(retry < num_tries):
dxl.write2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_GOAL_POSITION, value)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry +=1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry +=1
else:
return 0
print('[COMM_ERR] ' + str(get_caller()))
return -1
def PresentPos_1(self):
#Print current pos without goalpos
global DXL_MOVING_STATUS_THRESHOLD
#Read present position
self.dxl_present_position = dynamixel.read2ByteTxRx(
self.port_num, self.pro_ver, self.ID, self.addr_present)
self.dxl_comm_result = dynamixel.getLastTxRxResult(
self.port_num, self.pro_ver)
self.dxl_error = dynamixel.getLastRxPacketError(
self.port_num, self.pro_ver)
if self.dxl_comm_result != self.COMM_SUCCESS:
return (dynamixel.getTxRxResult(self.pro_ver,
self.dxl_comm_result))
elif self.dxl_error != 0:
return (dynamixel.getRxPacketError(self.pro_ver, self.dxl_error))
print("[ID:%03d] PresPos:%03d" % (self.ID, self.dxl_present_position))
return None
def reboot(self, id, port=0):
print("The LED should flicker")
dynamixel.reboot(self.port_num[port], self.protocol, id)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num[port],
self.protocol)
dxl_error = dynamixel.getLastRxPacketError(self.port_num[port],
self.protocol)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.protocol, dxl_comm_result))
return False
elif dxl_error != 0:
print(dynamixel.getRxPacketError(self.protocol, dxl_error))
return False
print("[ID:%03d] reboot Succeeded" % (id))
return True
def get_voltage(self, num_tries=RETRIES):
retry = 0
while (retry < num_tries):
voltage = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_PRESENT_VOLTAGE)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
return voltage / 10.0
print('[COMM_ERR] ' + str(get_caller()))
return None
def get_position(self, num_tries=RETRIES):
retry = 0
while (retry < num_tries):
pos = dxl.read2ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_PRESENT_POSITION)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
return self.value2rad(pos)
print('[COMM_ERR] ' + str(get_caller()))
return None
def disable_torque(self, num_tries=RETRIES):
retry = 0
while (retry < num_tries):
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_TORQUE_ENABLE, 0)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
return 0
raise Exception(
"disable_torque failed after {} tries".format(num_tries))
def disable_torque(self, num_tries=RETRIES):
retry = 0
while (retry < num_tries):
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_TORQUE_ENABLE, 0)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
return 0
print('[COMM_ERR] ' + str(get_caller()))
return -1
def set_torque_limit(self, torque, num_tries=RETRIES):
retry = 0
while (retry < num_tries):
dxl.write2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_TORQUE_LIMIT,
int(torque * 1023))
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
return 0
print('[COMM_ERR] ' + str(get_caller()))
return -1
def is_moving(self, num_tries=RETRIES):
retry = 0
while (retry < num_tries):
moving = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_MOVING)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
return moving
print('[COMM_ERR] ' + str(get_caller()))
return None
def set_id(id, new_id, baudrate):
"""Sets a servo ID.
:param int id: Servo ``ìd``
:param baudrate: Baudrate of the servo to be configured
:param int new_id: ``new_id`` to be configured
:return:
--``PORT_ERROR`` case it fails to open the port.
--``BAUDRATE_ERROR`` case it fails to change baudrate.
--``COMM_ERROR`` case there is a communication error.
--``HARDWARE_COMM_ERROR`` case there is a hardware communication error.
--``NONE`` case the operation succeeds."""
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Open port
if dynamixel.openPort(port_num):
print("Succeeded to open the port!")
else:
print("Failed to open the port!")
return PORT_ERROR
# Set port baudrate
if dynamixel.setBaudRate(port_num, baudrate):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
return BAUDRATE_ERROR
#Writes the new ID onto the register
dynamixel.write1ByteTxRx(port_num, PROTOCOL_1, id, ADDR_ID, new_id)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_1)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_1)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_1, dxl_comm_result))
return COMM_ERROR
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_1, dxl_error))
return HARDWARE_COMM_ERROR
else:
print("ID changed to: %s" % new_id)
time.sleep(0.2)
dynamixel.closePort(port_num)
def set_position(self, pos, num_tries=RETRIES):
value = self.rad2value(pos)
retry = 0
while (retry < num_tries):
dxl.write2ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_GOAL_POSITION, value)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
return 0
raise Exception("set_position failed after {} tries".format(num_tries))
def set_torque_limit(self, torque, num_tries=RETRIES):
retry = 0
while (retry < num_tries):
dxl.write2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_TORQUE_LIMIT,
int(torque * 1023))
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
return 0
raise Exception(
"set_torque_limit failed after {} tries".format(num_tries))
def get_speed(self, num_tries = RETRIES):
# returns normalized speed. multiply by max speed to get radians/s
retry = 0
while(retry < num_tries):
speed = dxl.read2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_SPEED)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry +=1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry +=1
else:
if(speed > 1024):
return (speed & 1023)/1023.0
else:
return -(speed & 1023)/1023.0
def _write(self, DXL_ID, angle):
dynamixel.write2ByteTxRx(
self.portHandler,
self.PROTOCOL_VERSION,
DXL_ID,
self.ADDR_MX_GOAL_POSITION,
2048 + int(angle / 0.088),
)
dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler,
self.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(self.portHandler,
self.PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print((dynamixel.getTxRxResult(self.PROTOCOL_VERSION,
self.dxl_comm_result)))
elif dxl_error != 0:
print((dynamixel.getRxPacketError(self.PROTOCOL_VERSION,
dxl_error)))
def _disable_torque(self, DXL_ID):
dynamixel.write2ByteTxRx(
self.portHandler,
self.PROTOCOL_VERSION,
DXL_ID,
self.ADDR_MX_TORQUE_ENABLE,
0,
)
dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler,
self.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(self.portHandler,
self.PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print((dynamixel.getTxRxResult(self.PROTOCOL_VERSION,
self.dxl_comm_result)))
elif dxl_error != 0:
print((dynamixel.getRxPacketError(self.PROTOCOL_VERSION,
dxl_error)))
def _set_moving_speed(self, DXL_ID, speed):
dynamixel.write2ByteTxRx(
self.portHandler,
self.PROTOCOL_VERSION,
DXL_ID,
self.ADDR_MX_SPEED,
int(speed * 1024 / 1000),
)
dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler,
self.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(self.portHandler,
self.PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print((dynamixel.getTxRxResult(self.PROTOCOL_VERSION,
self.dxl_comm_result)))
elif dxl_error != 0:
print((dynamixel.getRxPacketError(self.PROTOCOL_VERSION,
dxl_error)))
def set_goal_position(self, ids):
# for i, angle in ids.iteritems():
# self._write(i,angle)
for i, angle in list(ids.items()):
dxl_addparam_result = ctypes.c_ubyte(
dynamixel.groupSyncWriteAddParam(
self.groupSync,
i,
2048 + int(angle / 0.088),
self.LEN_MX_GOAL_POSITION,
)).value
dynamixel.groupSyncWriteTxPacket(self.groupSync)
dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler,
self.PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print((dynamixel.getTxRxResult(self.PROTOCOL_VERSION,
self.dxl_comm_result)))
dynamixel.groupSyncWriteClearParam(self.groupSync)
def get_load(self, num_tries = RETRIES):
# returns normalized load
retry = 0
while(retry < num_tries):
load = dxl.read2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_PRESENT_LOAD)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry +=1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry +=1
else:
if(load > 1024):
return (load & 1023)/1023.0
else:
return -(load & 1023)/1023.0
def set_mode(self,mode):
# 1 for wheel, 2 for joint
if((mode != 1) & (mode != 2)):
print('Error: Mode must be 1 or 2')
return
if(mode == 1):
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CW_LIMIT, 0)
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CCW_LIMIT, 0)
if(mode == 2):
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CW_LIMIT, 0)
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_CCW_LIMIT, 4095)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print(dxl.getTxRxResult(PROTOCOL, dcomm_result))
elif derror != 0:
print(dxl.getRxPacketError(PROTOCOL, derror))
self.mode = mode
def _read(self, DXL_ID):
dxl_present_position = dynamixel.read2ByteTxRx(
self.portHandler,
self.PROTOCOL_VERSION,
DXL_ID,
self.ADDR_MX_PRESENT_POSITION,
)
dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler,
self.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(self.portHandler,
self.PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print((dynamixel.getTxRxResult(self.PROTOCOL_VERSION,
self.dxl_comm_result)))
elif dxl_error != 0:
print((dynamixel.getRxPacketError(self.PROTOCOL_VERSION,
dxl_error)))
return (dxl_present_position - 2048) * 0.088
def Read_Value(self, motorId, read_addr, read_len):
# Read value
dxl_result = -1
if(read_len == 4):
dxl_result = dynamixel.read4ByteTxRx(self.port_num, self.proto_ver, motorId, read_addr)
elif(read_len == 2):
dxl_result = dynamixel.read2ByteTxRx(self.port_num, self.proto_ver, motorId, read_addr)
elif(read_len == 1):
dxl_result = dynamixel.read1ByteTxRx(self.port_num, self.proto_ver, motorId, read_addr)
else:
print('[ID:%03d]: invalid read length %d' %(motorId, read_len))
return dxl_result
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num, self.proto_ver)
dxl_error = dynamixel.getLastRxPacketError(self.port_num, self.proto_ver)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(self.proto_ver, dxl_error))
return dxl_result
def broadcast_ping(self, maxId, port=0, doPrint=False):
# Try to broadcast ping the Dynamixel
dynamixel.broadcastPing(self.port_num[port], self.protocol)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num[port], self.protocol)
if dxl_comm_result != COMM_SUCCESS:
if doPrint:
print(dynamixel.getTxRxResult(self.protocol, dxl_comm_result))
return False
if doPrint:
print("Detected Dynamixel : ")
nb_detected = 0
for id in range(1, int(maxId)+1):
if ctypes.c_ubyte(dynamixel.getBroadcastPingResult(self.port_num[port], self.protocol, id)).value:
nb_detected += 1
if doPrint:
print("[ID:%03d]" % (id))
if nb_detected == maxId:
return True
def ping(self, id, port=0, doPrint=False):
dxl_model_number = dynamixel.pingGetModelNum(self.port_num[port],
self.protocol, id)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num[port],
self.protocol)
dxl_error = dynamixel.getLastRxPacketError(self.port_num[port],
self.protocol)
if dxl_comm_result != COMM_SUCCESS:
if doPrint:
print(dynamixel.getTxRxResult(self.protocol, dxl_comm_result))
return False
elif dxl_error != 0:
if doPrint:
print(dynamixel.getRxPacketError(self.protocol, dxl_error))
return False
else:
if doPrint:
print("[ID:%03d] ping Succeeded. Dynamixel model number : %d" %
(id, dxl_model_number))
return True
def set_speed(self, speed, num_tries = RETRIES):
if(self.mode == 2):
value = int(1023 * abs(speed))
elif(self.mode == 1):
value = int(1023 * abs(speed))
if(speed > 0.0):
value = value + 1024
retry = 0
while(retry < num_tries):
dxl.write2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_MOVING_SPEED, value)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry +=1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry +=1
else:
return 0
raise Exception("set_speed failed after {} tries".format(num_tries))
def Set_Value(self, motorId, set_addr, set_len, value):
if (set_len == 4):
dynamixel.write4ByteTxRx(self.port_num, self.proto_ver, motorId,
set_addr, value)
elif (set_len == 2):
dynamixel.write2ByteTxRx(self.port_num, self.proto_ver, motorId,
set_addr, value)
elif (set_len == 1):
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, motorId,
set_addr, value)
else:
print('[ID:%03d]: invalid set length %d' % (motorId, set_len))
return
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num,
self.proto_ver)
dxl_error = dynamixel.getLastRxPacketError(self.port_num,
self.proto_ver)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(self.proto_ver, dxl_error))
def set_speed(self, speed, num_tries = RETRIES):
if(self.mode == 2):
value = int(1023*abs(speed))
elif(self.mode == 1):
value = int(1023*abs(speed))
if(speed>0.0):
value = value + 1024
retry = 0
while(retry < num_tries):
dxl.write2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_MOVING_SPEED, value)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry +=1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry +=1
else:
return 0
print('[COMM_ERR] ' + str(get_caller()))
return -1
def SetGoalPosition(self, Id, goalPose):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION,
Id, Registers.GOAL_POSITION, goalPose)
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION,
Id, Registers.GOAL_POSITION, goalPose)
if dynamixel.getLastTxRxResult(
self.port_num,
Registers.PROTOCOL_VERSION) != Registers.COMM_SUCCESS:
#dynamixel.printTxRxResult(Registers.PROTOCOL_VERSION, dynamixel.getLastTxRxResult(self.port_num, Registers.PROTOCOL_VERSION))
return False
elif dynamixel.getLastRxPacketError(
self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
else:
return True
except:
print "Oops! Problem writing..."
return False
def init_motors(ids_list, offsets):
global PORT_NUM, GROUP_NUM, IDS
IDS = ids_list
PORT_NUM = dynamixel.portHandler(DEVICENAME)
print(PORT_NUM)
# initialize PacketHandler structs
dynamixel.packetHandler()
GROUP_NUM = dynamixel.groupSyncWrite(PORT_NUM, PROTOCOL_VERSION,
ADDR_MX_GOAL_POSITION,
LEN_MX_GOAL_POSITION)
# open port
if not dynamixel.openPort(PORT_NUM):
print("Failed to open port!")
return -1
# set port baudrate
if not dynamixel.setBaudRate(PORT_NUM, BAUDRATE):
print("Failed to change the baudrate!")
return
# enable dynamixel torque
for DXL_ID in IDS:
dynamixel.write1ByteTxRx(PORT_NUM, PROTOCOL_VERSION, DXL_ID,
ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(PORT_NUM,
PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(PORT_NUM, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
return -1
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
return -1
if dynamixel.setBaudRate(port_num, BAUDRATE):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
# Read present baudrate of the controller
print("Now the controller baudrate is : %d" % (dynamixel.getBaudRate(port_num)))
# Try factoryreset
print("[ID:%03d] Try factoryreset : " % (DXL_ID))
dynamixel.factoryReset(port_num, PROTOCOL_VERSION, DXL_ID, OPERATION_MODE)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print("Aborted")
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
# Wait for reset
print("Wait for reset...")
sleep(2)
print("[ID:%03d] factoryReset Success!" % (DXL_ID))
# Set controller baudrate to dxl default baudrate
# Open port
if dynamixel.openPort(port_num):
print("Succeeded to open the port!")
else:
print("Failed to open the port!")
print("Press any key to terminate...")
getch()
quit()
# Set port baudrate
if dynamixel.setBaudRate(port_num, BAUDRATE):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
# Try to broadcast ping the Dynamixel
dynamixel.broadcastPing(port_num, PROTOCOL_VERSION)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
print("Detected Dynamixel : ")
for id in range(0, MAX_ID):
if ctypes.c_ubyte(dynamixel.getBroadcastPingResult(port_num, PROTOCOL_VERSION, id)).value:
print("[ID:%03d]" % (id))
# Close port
dynamixel.closePort(port_num)
