def Init_Port_And_Motors(self):
# Open port
if dynamixel.openPort(self.port_num):
print("Successfully opened the port!")
else:
print("Failed to open the port!")
print("Press any key to terminate...")
getch()
quit()
# Set port baudrate
if dynamixel.setBaudRate(self.port_num, self.baud_rate):
print("Successfully set the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
ADDR_PRO_TORQUE_ENABLE = 64
TORQUE_ENABLE = 1
dxl_comm_result = COMM_TX_FAIL
# Enable Dynamixel#1 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m1id, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
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))
else:
print("Dynamixel#1 has been successfully connected")
# Enable Dynamixel#2 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m2id, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
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))
else:
print("Dynamixel#2 has been successfully connected")
# Enable Dynamixel#3 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m3id, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
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))
else:
print("Dynamixel#3 has been successfully connected")
# Enable Dynamixel#4 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m4id, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
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))
else:
print("Dynamixel#4 has been successfully connected")
def Disable_Torque_Close_Port(self):
ADDR_PRO_TORQUE_ENABLE = 64
TORQUE_DISABLE = 0
dxl_comm_result = COMM_TX_FAIL
# Enable Dynamixel#1 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m1id,
ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
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))
else:
print("Dynamixel#1 has been successfully freed")
# Enable Dynamixel#2 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m2id,
ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
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))
else:
print("Dynamixel#2 has been successfully freed")
# Enable Dynamixel#3 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m3id,
ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
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))
else:
print("Dynamixel#3 has been successfully freed")
# Enable Dynamixel#4 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m4id,
ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
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))
else:
print("Dynamixel#4 has been successfully freed")
#close port
dynamixel.closePort(self.port_num)
def CheckTxRxResult(self):
dxl_result = dynamixel.getLastTxRxResult(self.port_num,
self.PROTOCOL_VERSION)
dxl_err = dynamixel.getLastRxPacketError(self.port_num,
self.PROTOCOL_VERSION)
if dxl_result != self.COMM_SUCCESS:
print dynamixel.getTxRxResult(self.PROTOCOL_VERSION, dxl_result)
return False
elif dxl_err != 0:
print dynamixel.getRxPacketError(self.PROTOCOL_VERSION, dxl_err)
return False
return True
def initDxlPro():
# set opmode to velocity control
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_PRO_ID, ADDR_PRO_OP_MODE, PRO_OPMODE_VEL)
# Enable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_PRO_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
resulttx = dynamixel.getTxRxResult(port_num, PROTOCOL_VERSION)
if resulttx != COMM_SUCCESS:
pass
elif dynamixel.getRxPacketError(port_num, PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getRxPacketError(port_num, PROTOCOL_VERSION))
else:
print("Dynamixel has been successfully connected")
def _set_moving_speed(self, DXL_ID, speed):
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 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
def set_baudrate(id, new_baudrate, baudrate):
"""Sets a servo baudrate.
:param int id: Servo ``ìd``
:param baudrate: Baudrate of the servo to be configured
:param int new_baudrate: ``new_baudrate`` 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
baudnum = {
1000000: 1,
500000: 3,
400000: 4,
250000: 7,
200000: 9,
115200: 16,
57600: 34,
19200: 103,
9600: 207
}
value = baudnum[new_baudrate]
# Set baudrate
dynamixel.write1ByteTxRx(port_num, PROTOCOL_1, id, ADDR_BAUDRATE, value)
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("Baudrate changed to : %s" % baudrate)
time.sleep(0.2)
dynamixel.closePort(port_num)
def set_compliance(self,
compliance_margin,
compliance_slope,
num_tries=RETRIES):
retry = 0
while (retry < num_tries):
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CW_COMPLIANCE_M, compliance_margin)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CCW_COMPLIANCE_M, compliance_margin)
dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL)
derror += dxl.getLastRxPacketError(self.port, PROTOCOL)
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CW_COMPLIANCE_S, compliance_slope)
dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL)
derror += dxl.getLastRxPacketError(self.port, PROTOCOL)
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CCW_COMPLIANCE_S, compliance_slope)
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_gains failed after {} tries".format(num_tries))
def set_mode(self, mode, num_tries=5):
# 1 for wheel, 2 for joint
if ((mode != 1) & (mode != 2)):
print_v('Error: Mode must be 1 or 2')
return -1
retry = 0
while (retry < num_tries):
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_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
self.mode = mode
return 0
raise Exception("set_mode failed after {} tries".format(num_tries))
def get_mode(self, num_tries=RETRIES):
mode = 0
retry = 0
while (retry < num_tries):
cw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CW_LIMIT)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
ccw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CCW_LIMIT)
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 ((cw_lim == 0) & (ccw_lim == 0)):
mode = 1
else:
mode = 2
return mode
raise Exception("get_mode failed after {} tries".format(num_tries))
def set_mode(self,mode, num_tries = 5):
# 1 for wheel, 2 for joint
if((mode != 1) & (mode != 2)):
print_v('Error: Mode must be 1 or 2')
return -1
retry = 0
while(retry < num_tries):
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_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry +=1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry +=1
else:
self.mode = mode
return 0
print('[COMM_ERR] ' + str(get_caller()))
return -1
def check_error(self, id):
error = dxl.getLastRxPacketError(self.port, self.protocol)
if error != 0:
print("[id: %d, %s]" %
(id, dxl.getRxPacketError(self.protocol, error)))
return False
return True
def get_mode(self, num_tries=RETRIES):
mode = 0
retry = 0
while (retry < num_tries):
cw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CW_LIMIT)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
ccw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CCW_LIMIT)
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 ((cw_lim == 0) & (ccw_lim == 0)):
mode = 1
else:
mode = 2
return mode
print('[COMM_ERR] ' + str(get_caller()))
return 0
def enable_torque(self):
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_TORQUE_ENABLE, 1)
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))
def set_torque_limit(self, torque):
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(dxl.getTxRxResult(PROTOCOL, dcomm_result))
elif derror != 0:
print(dxl.getRxPacketError(PROTOCOL, derror))
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 _write_speed(self, DXL_ID, speed):
dynamixel.write4ByteTxRx(self.portHandler, self.PROTOCOL_VERSION, DXL_ID, 44, 1023)
dynamixel.write4ByteTxRx(self.portHandler, self.PROTOCOL_VERSION, DXL_ID, self.ADDR_MX_SPEED, int(speed))
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(DXL_ID,dynamixel.getTxRxResult(self.PROTOCOL_VERSION, self.dxl_comm_result))
elif(dxl_error != 0):
print(DXL_ID,dynamixel.getRxPacketError(self.PROTOCOL_VERSION, dxl_error))
def get_voltage(self):
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(dxl.getTxRxResult(PROTOCOL, dcomm_result))
elif derror != 0:
print(dxl.getRxPacketError(PROTOCOL, derror))
return voltage/10.0
def get_temp(self):
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(dxl.getTxRxResult(PROTOCOL, dcomm_result))
elif derror != 0:
print(dxl.getRxPacketError(PROTOCOL, derror))
return temp
def set_position(self, pos):
value = self.rad2value(pos)
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(dxl.getTxRxResult(PROTOCOL, dcomm_result))
elif derror != 0:
print(dxl.getRxPacketError(PROTOCOL, derror))
def is_moving(self):
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(dxl.getTxRxResult(PROTOCOL, dcomm_result))
elif derror != 0:
print(dxl.getRxPacketError(PROTOCOL, derror))
return moving
def get_load(self):
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(dxl.getTxRxResult(PROTOCOL, dcomm_result))
elif derror != 0:
print(dxl.getRxPacketError(PROTOCOL, derror))
return (load/1023.0)-1.0
def get_position(self):
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(dxl.getTxRxResult(PROTOCOL, dcomm_result))
elif derror != 0:
print(dxl.getRxPacketError(PROTOCOL, derror))
return self.value2rad(pos)
def set_speed(ID):
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, ID,
ADDR_MX_MOVING_SPEED, MOVING_SPEED)
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))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
def set_gains(self, kp, ki, kd):
dxl.write2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_P_GAIN, kp)
dxl.write2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_I_GAIN, ki)
dxl.write2ByteTxRx(self.port, PROTOCOL, self.id, ADDR_D_GAIN, kd)
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))
def disable_torque(self):
dynamixel.write1ByteTxRx(servo.port_num, servo.PROTOCOL_VERSION, self.ID, servo.ADDR_PRO_TORQUE_ENABLE,
servo.TORQUE_DISABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(servo.port_num, servo.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(servo.port_num, servo.PROTOCOL_VERSION)
if servo.debug:
if dxl_comm_result != servo.COMM_SUCCESS:
print(dynamixel.getTxRxResult(servo.PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(servo.PROTOCOL_VERSION, dxl_error))
def write_velocity(self, dxl_goal_velocity):
dynamixel.write4ByteTxRx(servo.port_num, servo.PROTOCOL_VERSION, self.ID, servo.ADDR_PRO_GOAL_VELOCITY,
dxl_goal_velocity)
dxl_comm_result = dynamixel.getLastTxRxResult(servo.port_num, servo.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(servo.port_num, servo.PROTOCOL_VERSION)
if servo.debug:
if dxl_comm_result != servo.COMM_SUCCESS:
print(dynamixel.getTxRxResult(servo.PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(servo.PROTOCOL_VERSION, dxl_error))
def get_error_status(self):
error_status = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_HARDWARE_ERROR)
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))
return error_status
def get_mode(self):
mode = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CONTROL_MODE)
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))
return mode
def write_value(ID, final_pos):
# Write goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, ID,
ADDR_MX_GOAL_POSITION, final_pos)
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))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
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
if dynamixel.setBaudRate(port_num, FACTORYRST_DEFAULTBAUDRATE):
print("Succeed to change the controller baudrate to : %d" % (FACTORYRST_DEFAULTBAUDRATE))
else:
print("Failed to change the controller baudrate")
getch()
quit()