def SetCCWComplianceSlope(self, Id, ComSlopeCCW):
try:
dynamixel.write1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.CCW_COMPLIANCE_SLOPE, ComSlopeCCW)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
except :
print "Oops! Problem writing goal position..."
def SetHighestLimitTemperature(self, Id, highestLimitTemp):
try:
dynamixel.write1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.HIGHEST_LIMIT_TEMP, highestLimitTemp)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
except :
print "Oops! Problem writing..."
def SetAlarmShutdown(self, Id, alarmShutdown):
try:
dynamixel.write1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.ALARM_SHUTDOWN, alarmShutdown)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
except :
print "Oops! Problem writing goal position..."
def writeValue(self, address, value, size=1):
if(size==1):
dxl.write1ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \
address, value)
elif(size==2):
dxl.write2ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \
address, value)
def SetIGain(self, Id, IGain):
try:
dynamixel.write1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.CCW_COMPLIANCE_MARGIN, IGain)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
except :
print "Oops! Problem writing goal position..."
def enableTorques(self):
''' Enable torque for all motors connected
in this port.
'''
dxl.write1ByteTxRx(self.socket, PROTOCOL_VERSION, BROADCAST_ID, \
ADDR_MX_TORQUE_ENABLE, 1)
def SetTorqueEnable(self, Id, enable):
# Enable Dynamixel Torque
try:
dynamixel.write1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.TORQUE_ENABLE, enable)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
except :
print "Oops! Problem writing..."
def disableTorques(self):
''' Disables torque for all motors connected
to this port
'''
dxl.write1ByteTxRx(self.socket, PROTOCOL_VERSION, BROADCAST_ID, \
ADDR_MX_TORQUE_ENABLE, 0)
def set_gains(self,kp,ki,kd, num_tries = RETRIES):
retry = 0
while(retry < num_tries):
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_P_GAIN, kp)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_I_GAIN, ki)
dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL)
derror += dxl.getLastRxPacketError(self.port, PROTOCOL)
dxl.write1ByteTxRx(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_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_gains(self,kp,ki,kd, num_tries = RETRIES):
retry = 0
while(retry < num_tries):
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_P_GAIN, kp)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_I_GAIN, ki)
dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL)
derror += dxl.getLastRxPacketError(self.port, PROTOCOL)
dxl.write1ByteTxRx(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_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 disable_torque(self, ids):
for i in ids:
dynamixel.write1ByteTxRx(self.portHandler, self.PROTOCOL_VERSION, i, self.ADDR_MX_TORQUE_ENABLE, self.TORQUE_DISABLE)
def set_wheel_mode(self, ids, val = 1):
for i in ids:
dynamixel.write1ByteTxRx(self.portHandler,self.PROTOCOL_VERSION,i,11,val)
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
# Set port2 baudrate
if dynamixel.setBaudRate(port_num2, BAUDRATE):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
# Enable Dynamixel#1 Torque
dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID,
ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num1, 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))
else:
print("Dynamixel#1 has been successfully connected")
# Enable Dynamixel#2 Torque
dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID,
ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
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()
# Enable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
else:
print("Dynamixel has been successfully connected")
while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(ESC_ASCII_VALUE):
break
# Write goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index])
def motor_init(self):
# Read a keypress and return the resulting character as a byte string.
if os.name == 'nt':
import msvcrt
def getch():
return msvcrt.getch().decode()
else:
import sys, tty, termios
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
def getch():
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
# Control table address
ADDR_MX_TORQUE_ENABLE = 24 # 1 byte RW # Control table address is different in Dynamixel model
ADDR_MX_GOAL_POSITION = 30 # 2 bytes RW
ADDR_MX_PRESENT_POSITION = 36 # 2 bytes R
ADDR_MX_MOVING_SPEED = 32 # 2 bytes RW
ADDR_MX_PRESENT_SPEED = 38 # 2 bytes R
ADDR_MX_PRESENT_LOAD = 40 # 2 bytes R
ADDR_MX_PRESENT_VOLTAGE = 42 # 1 byte R
ADDR_MX_GOAL_ACCELERATION = 73 # 1 byte RW
ADDR_MX_MAX_TORQUE = 14 # 2 bytes RW
# PID Controller Address
ADDR_MX_DERIVATIVE_GAIN = 26 # 1 byte RW
ADDR_MX_INTEGRAL_GAIN = 27 # 1 byte RW
ADDR_MX_PROPORTIONAL_GAIN = 28 # 1 byte RW
# Protocol version
PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel
# Default setting
DXL_ID = [1, 2, 3, 4]
BAUDRATE = 1000000
DEVICENAME = "COM7".encode(
'utf-8') # Check which port is being used on your controller
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
# Resolution is 0.088 degree / Position 0~4095 /
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_MOVING_STATUS_THRESHOLD = 5 # Dynamixel moving status threshold
ESC_ASCII_VALUE = 0x1b # for ESC key to escape out from the operation
COMM_SUCCESS = 0 # Communication Success result value
COMM_TX_FAIL = -1001 # Communication Tx Failed
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
index = 0
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_error = 0 # Dynamixel error
dxl_present_position = 0 # Present position
#set position P gain, I gain, D gain - all 1 byte - P only 254 for unit communication
# Motor 1
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0],
ADDR_MX_DERIVATIVE_GAIN, 254)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0],
ADDR_MX_INTEGRAL_GAIN, 30)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0],
ADDR_MX_PROPORTIONAL_GAIN, 65)
# Motor 2
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1],
ADDR_MX_DERIVATIVE_GAIN, 254)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1],
ADDR_MX_INTEGRAL_GAIN, 50)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1],
ADDR_MX_PROPORTIONAL_GAIN, 120)
# Motor 3
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2],
ADDR_MX_DERIVATIVE_GAIN, 254)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2],
ADDR_MX_INTEGRAL_GAIN, 50)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2],
ADDR_MX_PROPORTIONAL_GAIN, 120)
# Motor 4
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3],
ADDR_MX_DERIVATIVE_GAIN, 254)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3],
ADDR_MX_INTEGRAL_GAIN, 50)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3],
ADDR_MX_PROPORTIONAL_GAIN, 120)
for id in DXL_ID:
# Enable Dynamixel Torque - 1byte
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id,
ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, id,
ADDR_MX_MAX_TORQUE, 1024)
dxl_comm_result = dynamixel.getLastTxRxResult(
port_num, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num,
PROTOCOL_VERSION)
# Initialize Goal Position 2048
dxl_goal_position_init = 2048
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3],
ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3],
ADDR_MX_MAX_TORQUE, 1024)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3],
ADDR_MX_GOAL_POSITION, 2500)
time.sleep(5)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0],
ADDR_MX_GOAL_POSITION, 2048)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1],
ADDR_MX_GOAL_POSITION, 2048)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2],
ADDR_MX_GOAL_POSITION, 2048)
time.sleep(3)
# Disable Dynamixel Torque
for id in DXL_ID:
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id,
ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE)
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))
# Close port
dynamixel.closePort(port_num)
def EnableTorque(self, enabled=True):
value = self.TORQUE_ENABLE if enabled else self.TORQUE_DISABLE
dynamixel.write1ByteTxRx(self.port_num, self.PROTOCOL_VERSION,
self.DXL_ID, self.ADDR_PRO_TORQUE_ENABLE,
value)
self.CheckTxRxResult()
def __write_byte(self, servo_id, address, data):
dynamixel.write1ByteTxRx(self.__port_num, PROTOCOL, servo_id, address, data)
self.__verify_last_message()
def set_pid_gain(id, d_gain, i_gain, p_gain, baudrate):
"""Sets the PID Gains.
:param int id: Servo ``ìd``
:param baudrate: Baudrate of the servo to be configured
:param int d_gain: D Gain
:param int i_gain: I Gain
:param int p_gain: P Gain
: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
# D gain config
dynamixel.write1ByteTxRx(port_num, PROTOCOL_1, id, ADDR_D_GAIN, d_gain)
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("D gain set")
time.sleep(0.2)
# I gain config
dynamixel.write1ByteTxRx(port_num, PROTOCOL_1, id, ADDR_I_GAIN, i_gain)
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("I gain set")
time.sleep(0.2)
# P gain config
dynamixel.write1ByteTxRx(port_num, PROTOCOL_1, id, ADDR_P_GAIN, p_gain)
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("P gain set")
time.sleep(0.2)
dynamixel.closePort(port_num)
# if not (abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD):
# break
# Change goal position
# if index == 0:
# index = 1
# else:
# index = 0
rate.sleep()
# Disable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_PRO_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_106_ID, ADDR_106_TORQUE_ENABLE, TORQUE_DISABLE)
# Close port
dynamixel.closePort(port_num)
if __name__ == '__main__':
try:
init()
initDxl106()
initDxlPro()
control()
except rospy.ROSInterruptException:
# Disable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_PRO_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
def disable_bot(ID):
# Disable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, ID,
ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
else:
print("Failed to open the port!")
print("Press any key to terminate...")
getch()
quit()
# Seta a baud rate da porta
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()
# Set baudrate
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_ID,
NEW_DXL_ID)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(
PROTOCOL_VERSION,
dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
else:
print("ID CHANGED FROM %s TO : %s" % (DXL_ID, NEW_DXL_ID))
dynamixel.closePort(port_num)
def torque_ctrl(): dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, 1, ADDR_MX_TORQUE_CONT, 1) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
def enableTorques(self):
''' Enable torque for all motors connected
in this port.
'''
dxl.write1ByteTxRx(self.socket, PROTOCOL_VERSION, BROADCAST_ID, \
ADDR_MX_TORQUE_ENABLE, 1)
def motor_operate(self, theta1_vp, theta2_vp, e, tx, ty, div_pt_x_vp, div_pt_y_vp):
# ta1_vp and theta2_vp from list to float type
theta1_vp = theta1_vp.tolist()
theta2_vp = theta2_vp.tolist()
for i in range(len(theta1_vp)):
theta1_vp[i] = float(theta1_vp[i][0])
theta2_vp[i] = float(theta2_vp[i][0])
# Read a keypress and return the resulting character as a byte string.
if os.name == 'nt':
import msvcrt
def getch():
return msvcrt.getch().decode()
else:
import sys, tty, termios
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
def getch():
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
# Control table address
ADDR_MX_TORQUE_ENABLE = 24 # 1 byte RW # Control table address is different in Dynamixel model
ADDR_MX_GOAL_POSITION = 30 # 2 bytes RW
ADDR_MX_PRESENT_POSITION = 36 # 2 bytes R
ADDR_MX_MOVING_SPEED = 32 # 2 bytes RW
ADDR_MX_PRESENT_SPEED = 38 # 2 bytes R
ADDR_MX_PRESENT_LOAD = 40 # 2 bytes R
ADDR_MX_PRESENT_VOLTAGE = 42 # 1 byte R
ADDR_MX_GOAL_ACCELERATION = 73 # 1 byte RW
ADDR_MX_MAX_TORQUE = 14 # 2 bytes RW
# PID Controller Address
ADDR_MX_DERIVATIVE_GAIN = 26 # 1 byte RW
ADDR_MX_INTEGRAL_GAIN = 27 # 1 byte RW
ADDR_MX_PROPORTIONAL_GAIN = 28 # 1 byte RW
# Protocol version
PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel
# Default setting
DXL_ID = [1, 2, 3, 4]
BAUDRATE = 1000000
DEVICENAME = "COM7".encode('utf-8') # Check which port is being used on your controller
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
# Resolution is 0.088 degree / Position 0~4095 /
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_MOVING_STATUS_THRESHOLD = 5 # Dynamixel moving status threshold
ESC_ASCII_VALUE = 0x1b # for ESC key to escape out from the operation
COMM_SUCCESS = 0 # Communication Success result value
COMM_TX_FAIL = -1001 # Communication Tx Failed
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
index = 0
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_error = 0 # Dynamixel error
dxl_present_position = 0 # Present position
theta1 = []
for i in range(len(theta1_vp)):
theta1.append(int((180 * theta1_vp[i]) / (0.088 * np.pi)) + theta1_offset - 2048 + dynamixel_offset_90)
print('=====================theta1 =======================')
print(theta1)
theta2 = []
for i in range(len(theta2_vp)):
theta2.append(int((180 * theta2_vp[i]) / (0.088 * np.pi)) + theta2_offset - 2048 + 2048)
print('=====================theta2=======================')
print(theta2)
theta3 = [theta3_offset] * len(theta2_vp)
#theta4 = [2048]
# 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()
#set position P gain, I gain, D gain - all 1 byte - P only 254 for unit communication
# Motor 1
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_DERIVATIVE_GAIN, 254)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_INTEGRAL_GAIN, 30)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PROPORTIONAL_GAIN, 80)
# Motor 2
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_DERIVATIVE_GAIN, 254)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_INTEGRAL_GAIN, 50)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_PROPORTIONAL_GAIN, 120)
# Motor 3
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_DERIVATIVE_GAIN, 254)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_INTEGRAL_GAIN, 50)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_PROPORTIONAL_GAIN, 120)
# Motor 4
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_DERIVATIVE_GAIN, 254)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_INTEGRAL_GAIN, 50)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_PROPORTIONAL_GAIN, 120)
for id in DXL_ID:
# Enable Dynamixel Torque - 1byte
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_MAX_TORQUE, 1024)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
# Initialize Goal Position 2048
dxl_goal_position_init = 2048
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_MAX_TORQUE, 1024)
# Estimate time
start_time = time.time()
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, 2200)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, 2200)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, 2048)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2500)
time.sleep(2)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, theta1[0])
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, theta2[0])
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, theta3[0])
time.sleep(5)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2038)
time.sleep(4)
while not (len(theta1) - index) == 0:
# Write goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, theta1[index])
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, theta2[index])
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, theta3[index])
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2038)
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))
# write moving speed
for id in DXL_ID:
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_MOVING_SPEED, 50)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_MOVING_SPEED, 20)
while 1:
# Read present position
dxl_present_position_theta1 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0],
ADDR_MX_PRESENT_POSITION)
dxl_present_position_theta2 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1],
ADDR_MX_PRESENT_POSITION)
dxl_present_position_theta3 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2],
ADDR_MX_PRESENT_POSITION)
dxl_present_position_theta4 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3],
ADDR_MX_PRESENT_POSITION)
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))
# read voltage
dxl_present_voltage = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0],
ADDR_MX_PRESENT_VOLTAGE)
# read load
dxl_present_load = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PRESENT_LOAD)
# read present speed
dxl_present_vel = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PRESENT_SPEED)
#print("[ID:%03d] GoalPos:%03d PresPos:%03d PresVolt:%03d PresLoad:%03d PresVel:%03d" % (
#DXL_ID[0], theta2[index], dxl_present_position_theta2,
#dxl_present_voltage, dxl_present_load, dxl_present_vel))
#print('e', e.is_set())
if e.is_set() == 0:
pass
elif e.is_set() != 0:
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2500)
time.sleep(5)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, 2200)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, 2200)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, 2048)
#contour
cam_paper = cv2.VideoCapture(0)
ret_paper, frame_paper = cam_paper.read()
frame_paper = cv2.flip(frame_paper, -1)
cv2.imshow('frame', frame_paper)
cv2.waitKey(1)
cont_paper = contour_paper.ContourPaper()
tx, ty = cont_paper.cont_paper(frame_paper)
print('translated tx and ty', tx, ty)
#paper move
#new space for current theta1 and theta2 to return IKINE
new_theta1 = np.zeros([len(theta1) - index, 1])
new_theta2 = np.zeros([len(theta2) - index, 1])
#new space of current theta1 and theta2 in degree
theta1_deg = np.zeros([])
theta2_deg = np.zeros([])
theta1_deg = np.delete(theta1_deg, 0)
theta2_deg = np.delete(theta2_deg, 0)
#current theta1 and theta2, remove past theta value
current_theta1 = theta1[index: ]
current_theta2 = theta2[index: ]
div_pt_x_vp = div_pt_x_vp[index: ]
div_pt_y_vp = div_pt_y_vp[index: ]
#print('x position, ', div_pt_x_vp)
#print('imhere1')
for i in range(len(current_theta1)):
theta1_deg = np.append(theta1_deg, ((current_theta1[i] - theta1_offset + 2048 - dynamixel_offset_90) * np.pi * 0.088 / 180))
theta2_deg = np.append(theta2_deg, ((current_theta2[i] - theta2_offset) * np.pi * 0.088 / 180))
IK = ikine.InverseKinematics()
for i in range(len(current_theta2)):
#print('theta1_deg', theta1_deg[i])
new_theta1[i], new_theta2[i] = IK.ikine(np.int(div_pt_x_vp[i]), np.int(div_pt_y_vp[i]), tx, ty)
#print('new_theta1, ', new_theta1)
new_theta1 = new_theta1.tolist()
new_theta2 = new_theta2.tolist()
for i in range(len(new_theta1)):
new_theta1[i] = float(new_theta1[i][0])
new_theta2[i] = float(new_theta2[i][0])
theta1 = []
for i in range(len(new_theta1)):
theta1.append(int((180 * new_theta1[i]) / (0.088 * np.pi)) + theta1_offset - 2048 + dynamixel_offset_90)
print('=====================theta1 =======================')
print(theta1)
theta2 = []
for i in range(len(new_theta2)):
theta2.append(int((180 * new_theta2[i]) / (0.088 * np.pi)) + theta2_offset - 2048 + 2048)
print('=====================theta2=======================')
print(theta2)
theta3 = [theta3_offset] * len(new_theta2)
index = 0
time.sleep(5)
break
else:
pass
if not ((abs(theta1[index] - dxl_present_position_theta1) > DXL_MOVING_STATUS_THRESHOLD) | ((abs(theta2[index] - dxl_present_position_theta2)) > DXL_MOVING_STATUS_THRESHOLD) | ((abs(theta3[index] - dxl_present_position_theta3)) > DXL_MOVING_STATUS_THRESHOLD)) :
break
# check event
#ABOUT TX and TY -> IKINE -> New theta1 and theta2
init_tx = tx
init_ty = ty
if e.is_set() != 0:
print('error')
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, theta1[0])
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, theta2[0])
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, theta3[0])
time.sleep(5)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2038)
time.sleep(3)
e.clear()
else:
pass
index += 1
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2500)
time.sleep(5)
# Disable Dynamixel Torque
for id in DXL_ID:
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE)
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))
# Close port
dynamixel.closePort(port_num)
return tx, ty
def initDynamixel(self):
# DIFFERENT MODES
# JOIN Mode
# MOVING SPEED 0-1023 (0.114 rpm) (1023 - 117.07 rpm)
# GOAL POSITION 0-4095 (0.088 degree)
# MULTI-TURN Mode
# MOVING SPEED 0-1023
# GOAL POSITION -28672 - 28672 (0.088 * RESOLUTION DIVIDER)
# WHEEL Mode
# MOVING SPEED 0-2047 (1024-2047 is CW, 0-1023 is CCW) 10th bit flips direction
# NO GOAL POSITION
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
self.port_num = dynamixel.portHandler(self.DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
self.index = 0
self.dxl_comm_result = self.COMM_TX_FAIL # Communication result
self.dxl_goal_position = [
self.DXL_MINIMUM_POSITION_VALUE, self.DXL_MAXIMUM_POSITION_VALUE
] # Goal position
self.dxl_error = 0 # Dynamixel error
self.dxl_present_position = 0 # Present position
# Open port
if dynamixel.openPort(self.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(self.port_num, self.BAUDRATE):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
#getch()
#quit()
# Enable Dynamixel Torque
dynamixel.write1ByteTxRx(self.port_num, self.PROTOCOL_VERSION,
self.DXL_ID, self.ADDR_MX_TORQUE_ENABLE,
self.TORQUE_ENABLE)
if dynamixel.getLastTxRxResult(
self.port_num, self.PROTOCOL_VERSION) != self.COMM_SUCCESS:
dynamixel.printTxRxResult(
self.PROTOCOL_VERSION,
dynamixel.getLastTxRxResult(self.port_num,
self.PROTOCOL_VERSION))
elif dynamixel.getLastRxPacketError(self.port_num,
self.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
self.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
self.PROTOCOL_VERSION))
else:
print("Dynamixel has been successfully connected")
# Set Speed
dynamixel.write2ByteTxRx(self.port_num, self.PROTOCOL_VERSION,
self.DXL_ID, self.ADDR_MX_MOVING_SPEED, 200)
if dynamixel.getLastTxRxResult(
self.port_num, self.PROTOCOL_VERSION) != self.COMM_SUCCESS:
dynamixel.printTxRxResult(
self.PROTOCOL_VERSION,
dynamixel.getLastTxRxResult(self.port_num,
self.PROTOCOL_VERSION))
elif dynamixel.getLastRxPacketError(self.port_num,
self.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
self.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
self.PROTOCOL_VERSION))
else:
print("Dynamixel speed has been set successfully")
print('Dynamixel initialized')
def disableTorque(self):
''' Disables torque in this joint
'''
dxl.write1ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \
ADDR_MX_TORQUE_ENABLE, 0)
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()
print("Setting operating mode to position")
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID,
ADDR_X_OPERATING_MODE, X_OPERATING_MODE_POSITION)
# Enable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID,
ADDR_PRO_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))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
else:
print("Dynamixel has been successfully connected")
while 1:
print("Press any key to continue! (or press ESC to quit!)")
def disableTorque(self):
''' Disables torque in this joint
'''
dxl.write1ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \
ADDR_MX_TORQUE_ENABLE, 0)
def disable_torque(dxl_id):
# Disable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, dxl_id,
ADDR_X_TORQUE_ENABLE, TORQUE_DISABLE)
else:
print("Failed to change the controller baudrate")
getch()
quit()
# Read Dynamixel baudnum
dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
else:
print("[ID:%03d] Dynamixel baudnum is now : %d" % (DXL_ID, dxl_baudnum_read))
# Write new baudnum
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE, NEW_BAUDNUM)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
else:
print("[ID:%03d] Set Dynamixel baudnum to : %d" % (DXL_ID, NEW_BAUDNUM))
# Set port baudrate to BAUDRATE
if dynamixel.setBaudRate(port_num, BAUDRATE):
print("Succeed to change the controller baudrate to : %d" % (BAUDRATE))
else:
print("Failed to change the controller baudrate")
getch()
quit()
def set_secondary_id(self, ids, secid):
for id in ids:
dxl.write1ByteTxRx(self.port, self.protocol, id, ADDR_SECONDARY_ID,
secid)
self.check_result(id)
self.check_error(id)
print("Press any key to terminate...")
getch()
quit()
# Set port2 baudrate
if dynamixel.setBaudRate(port_num2, BAUDRATE):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
# Enable Dynamixel#1 Torque
dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num1, 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))
else:
print("Dynamixel#1 has been successfully connected")
# Enable Dynamixel#2 Torque
dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
def set_op_mode(self, ids, mode=3):
for id in ids:
dxl.write1ByteTxRx(self.port, self.protocol, id, ADDR_OP_MODE,
mode)
self.check_result(id)
self.check_error(id)
def reverse_slave(id, reverse_mode_enable, slave_mode_enable, baudrate):
"""Sets the drive mode.
:param int id: Servo ``ìd``
:param baudrate: Baudrate of the servo to be configured
:param int reverse_mode_enable: Reverse mode checkbox state
:param int slave_mode_enable: Slave mode checkbox state
: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
slave_binary = 0x02
reverse_binary = 0x01
drive_mode_byte = 0x00
if reverse_mode_enable == 2:
drive_mode_byte = reverse_binary + drive_mode_byte
else:
drive_mode_byte = drive_mode_byte
if slave_mode_enable == 2:
drive_mode_byte = slave_binary + drive_mode_byte
else:
drive_mode_byte = drive_mode_byte
# Set drive mode
dynamixel.write1ByteTxRx(port_num, PROTOCOL_1, id, ADDR_DRIVE_MODE,
drive_mode_byte)
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("Drive mode changed")
time.sleep(0.2)
dynamixel.closePort(port_num)
def set_torque_status(self, ids, value):
for id in ids:
dxl.write1ByteTxRx(self.port, self.protocol, id,
ADDR_TORQUE_ENABLE, value)
self.check_result(id)
self.check_error(id)
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")
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit)
# Velocity Limit #2
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit)
# Velocity Limit #3
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL3_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit)
# Velocity Limit #4
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL4_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit)
# Velocity Limit #5
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL5_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit)
# Velocity Limit #6
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL6_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit)
# Velocity Limit #7
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL7_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit)
# Enable Dynamixel#1 Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_XM430_TORQUE_ENABLE, TORQUE_ENABLE)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
else:
print("Dynamixel#1 has been successfully connected")
# Enable Dynamixel#2 Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_XM430_TORQUE_ENABLE, TORQUE_ENABLE)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
else:
print("Dynamixel#2 has been successfully connected")
getch()
quit()
# Read Dynamixel baudnum
dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE)
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))
else:
print("[ID:%03d] Dynamixel baudnum is now : %d" % (DXL_ID, dxl_baudnum_read))
# Write new baudnum
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE, NEW_BAUDNUM)
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))
else:
print("[ID:%03d] Set Dynamixel baudnum to : %d" % (DXL_ID, NEW_BAUDNUM))
# Set port baudrate to BAUDRATE
if dynamixel.setBaudRate(port_num, BAUDRATE):
print("Succeed to change the controller baudrate to : %d" % (BAUDRATE))
else:
print("Failed to change the controller baudrate")
getch()
def motor_disable(self, theta1_vp, theta2_vp):
if os.name == 'nt':
import msvcrt
def getch():
return msvcrt.getch().decode()
else:
import sys, tty, termios
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
def getch():
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
# Control table address
ADDR_MX_TORQUE_ENABLE = 24 # 1 byte RW # Control table address is different in Dynamixel model
# Protocol version
PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel
# Default setting
DXL_ID = [1, 2, 3, 4]
BAUDRATE = 1000000
DEVICENAME = "COM7".encode(
'utf-8') # Check which port is being used on your controller
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
# Resolution is 0.088 degree / Position 0~4095 /
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_MOVING_STATUS_THRESHOLD = 3 # Dynamixel moving status threshold
ESC_ASCII_VALUE = 0x1b # for ESC key to escape out from the operation
COMM_SUCCESS = 0 # Communication Success result value
COMM_TX_FAIL = -1001 # Communication Tx Failed
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
index = 0
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_error = 0 # Dynamixel error
dxl_present_position = 0 # Present position
# Disable Dynamixel Torque
for id in DXL_ID:
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id,
ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE)
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))
# Close port
dynamixel.closePort(port_num)
