def Setup(self):
# 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()
#Open port
if dynamixel.openPort(self.port_num):
print 'Opened a port:', self.DEVICENAME
else:
print 'Failed to open a port:', self.DEVICENAME
self.port_num = None
return False
#Set port baudrate
if dynamixel.setBaudRate(self.port_num, self.BAUDRATE):
print 'Changed the baud rate to:', self.BAUDRATE
else:
print 'Failed to change the baud rate to:', self.BAUDRATE
return False
self.SetOpMode(self.OP_MODE_POSITION)
self.EnableTorque()
def __init__(self, port_name):
self.__port_num = dynamixel.portHandler(port_name.encode('utf-8'))
dynamixel.packetHandler()
if not dynamixel.openPort(self.__port_num):
raise IOError("Failed to open port")
if not dynamixel.setBaudRate(self.__port_num, 1000000):
raise IOError("Failed to set baud rate")
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 __init__(self, DeviceName):
self.portHandler = dynamixel.portHandler(DeviceName)
self.packetHandler = dynamixel.packetHandler()
self.dxl_comm_result = COMM_TX_FAIL
self.dxl_error = 0
self.ADDR_MX_TORQUE_ENABLE = 24 # Control table address is different in Dynamixel model
self.ADDR_MX_GOAL_POSITION = 30
self.ADDR_MX_PRESENT_POSITION = 36
self.ADDR_MX_SPEED = 32
# Protocol version
self.PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel
# Default setting
self.BAUDRATE = 1000000
self.TORQUE_ENABLE = 1 # Value for enabling the torque
self.TORQUE_DISABLE = 0 # Value for disabling the torque
self.DXL_MINIMUM_POSITION_VALUE = 100 # Dynamixel will rotate between this value
self.DXL_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
self.DXL_MOVING_STATUS_THRESHOLD = 10
if (dynamixel.openPort(self.portHandler)):
print "Port open done"
else:
print("Failed to open port")
quit()
if (dynamixel.setBaudRate(self.portHandler, self.BAUDRATE)):
print "Change baudrate succeeded"
else:
print "Cannot add baudrate"
quit()
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)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_1, dynamixel.getLastTxRxResult(port_num, PROTOCOL_1))
return COMM_ERROR
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1) != 0:
dynamixel.printRxPacketError(PROTOCOL_1, dynamixel.getLastRxPacketError(port_num, PROTOCOL_1))
return HARDWARE_COMM_ERROR
else:
print("Drive mode changed")
time.sleep(0.2)
def Setup(self):
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
self.port_num = dynamixel.portHandler(self.DevName)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
#Open port
if dynamixel.openPort(self.port_num):
print 'Opened a port:', self.DevName
else:
print 'Failed to open a port:', self.DevName
self.port_num = None
return False
#Set port baudrate
if dynamixel.setBaudRate(self.port_num, int(self.Baudrate)):
print 'Changed the baud rate to:', self.Baudrate
else:
print 'Failed to change the baud rate to:', self.Baudrate
return False
self.SetOpMode(self.OP_MODE[self.OpMode])
#self.EnableTorque()
return True
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 __init__(self, portName, baudrate):
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
self.port_num = dynamixel.portHandler(portName)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
#Open port
if dynamixel.openPort(self.port_num):
print("HardwareTools-Dynamixel.->Succeeded to open the port!")
else:
print("HardwareTools-Dynamixel.->Failed to open the port!")
print("HardwareTools-Dynamixel.->Press any key to terminate...")
getch()
quit()
# Set port baudrate
if dynamixel.setBaudRate(self.port_num, baudrate):
print("HardwareTools-Dynamixel.->Succeeded to change the baudrate!")
else:
print("HardwareTools-Dynamixel.->Failed to change the baudrate!")
print("HardwareTools-Dynamixel.->Press any key to terminate...")
getch()
quit()
def __init__(self, portName, baudrate):
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
self.port_num = dynamixel.portHandler(portName)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
#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, baudrate):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
def set_angle_limit(id,cw_angle_limit, ccw_angle_limit,baudrate):
"""Configure the angle limits of a servo.
:param int id: Servo ``ìd``
:param int cw_angle_limit: Clockwise angle limit to be configured
:param int ccw_angle_limit: Counter-clockwise angle limit to be configured
:param baudrate: Baudrate of the servo 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
#Write CW angle limit
dynamixel.write2ByteTxRx(port_num, PROTOCOL_1 , id, ADDR_CW_ANGLE_LIMIT, cw_angle_limit)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 ) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_1 , dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 ))
return COMM_ERROR
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 ) != 0:
dynamixel.printRxPacketError(PROTOCOL_1 , dynamixel.getLastRxPacketError(port_num,PROTOCOL_1 ))
return HARDWARE_COMM_ERROR
else:
print("CW angle changed to: %s" % cw_angle_limit)
time.sleep(0.5)
# Write CCW angle limit
dynamixel.write2ByteTxRx(port_num, PROTOCOL_1 , id, ADDR_CCW_ANGLE_LIMIT, ccw_angle_limit)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 ) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_1 , dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 ))
return COMM_ERROR
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 ) != 0:
dynamixel.printRxPacketError(PROTOCOL_1 , dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 ))
return HARDWARE_COMM_ERROR
else:
print("CCW angle changed to: %s" % ccw_angle_limit)
time.sleep(0.5)
def __init__(self, baudrate=1000000, port='/dev/ttyUSB0', protocol=1):
self.baudrate = baudrate
self.port = dxl.portHandler(port.encode('utf-8'))
self.protocol = protocol
if self.protocol == 1:
chain = protocol1_0.Chain(port=self.port)
elif self.protocol == 2:
chain = protocol2_0.Chain(port=self.port)
dxl.packetHandler()
self.connect()
def __init__(self, baudrate=1000000, port='/dev/ttyUSB0', protocol=2):
self.baudrate = baudrate
self.port = dxl.portHandler(port.encode('utf-8'))
self.protocol = protocol
self.resolution = MX_RESOLUTION # MX-28 Resolution
self.groupwrite = dxl.groupSyncWrite(self.port, self.protocol,
ADDR_GOAL_POS, LEN_GOAL_POSITION)
self.groupread = dxl.groupSyncRead(self.port, self.protocol,
ADDR_PRES_POS, LEN_PRESENT_POSITION)
dxl.packetHandler()
self.connect()
def set_torque_max(id, percentage, baudrate):
"""Sets a servo max torque.
:param int id: Servo ``ìd``
:param baudrate: Baudrate of the servo to be configured
:param int percentage: Torque percentage
: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
#Converting percentage to bit value (check dynamixel e-manual for info)
if percentage == 100:
value = 1023
else:
value = int(percentage / 0.0977517107)
# Write goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_1, id, ADDR_MAX_TORQUE, 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("Torque set to %s " % percentage)
time.sleep(0.2)
dynamixel.closePort(port_num)
def enable_port():
port_num = dynamixel.portHandler(DEVICENAME)
# Open port
if dynamixel.openPort(port_num):
print("[+] Succeeded to open the port!")
else:
print("[-] Failed to open the port!")
quit()
# Set port baudrate
if dynamixel.setBaudRate(port_num, BAUDRATE):
print("[+] Succeeded to change the baudrate!")
else:
print("[-] Failed to change the baudrate!")
quit()
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 factory_reset(id, baudrate):
"""Resets a servo to factory config.
:param int id: Servo ``ìd``
:param baudrate: Baudrate of the servo 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
dynamixel.factoryReset(port_num, PROTOCOL_1, id, OPERATION_MODE)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1) != COMM_SUCCESS:
print("Aborted")
dynamixel.printTxRxResult(
PROTOCOL_1, dynamixel.getLastTxRxResult(port_num, PROTOCOL_1))
return COMM_ERROR
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1) != 0:
dynamixel.printRxPacketError(
PROTOCOL_1, dynamixel.getLastRxPacketError(port_num, PROTOCOL_1))
return HARDWARE_COMM_ERROR
# Wait for reset
time.sleep(2)
def __init__(self, commPort="/dev/ttyUSB0", baudnum=1):
''' The argument commPort should be
the path to the serial device.
The constructor optionally takes
a baudnum argument:
baudrate = 2Mbps / (baudnum + 1)
If no baudnum is provided, then the
default is 1, resulting 1Mbps
'''
self.commPort = commPort
self.baudnum = baudnum
self.baudRate = 2000000 / (baudnum + 1)
self.socket = dxl.portHandler(self.commPort.encode('utf-8'))
dxl.packetHandler()
if dxl.openPort(self.socket):
print("Port Open Success")
if dxl.setBaudRate(self.socket, self.baudRate):
print("Port Baud Set Success")
def __init__(
self,
# Check which port is being used on your controller
device_name="COM5".encode('utf-8'),
# baud rate
baud_rate=1000000,
# motor ids
m1id=100,
m2id=101,
m3id=102,
m4id=103,
#protocol ver
proto_ver=2,
#SyncRead Addr
read_addr=126,
#SyncRead Len
read_len=2):
self.baud_rate = baud_rate
self.device_name = device_name
self.m1id = m1id
self.m2id = m2id
self.m3id = m3id
self.m4id = m4id
self.proto_ver = proto_ver
self.read_addr = read_addr
self.read_len = read_len
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
self.port_num = dynamixel.portHandler(device_name)
dynamixel.setPacketTimeoutMSec(1)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Initialize Groupsyncread Structs for Current
self.groupread_num = dynamixel.groupSyncRead(self.port_num, proto_ver,
read_addr,
read_len) #0, 148
dt = datetime.datetime.now()
self.timestamp0 = dt.minute * 60000000 + dt.second * 1000000 + dt.microsecond
self.Init_Port_And_Motors()
self.Init_Param_Storage()
def __init__(self, commPort="/dev/ttyUSB0", baudnum = 1):
''' The argument commPort should be
the path to the serial device.
The constructor optionally takes
a baudnum argument:
baudrate = 2Mbps / (baudnum + 1)
If no baudnum is provided, then the
default is 1, resulting 1Mbps
'''
self.commPort = commPort
self.baudnum = baudnum
self.baudRate = 2000000/(baudnum+1)
self.socket = dxl.portHandler(self.commPort.encode('utf-8'))
dxl.packetHandler()
if dxl.openPort(self.socket):
print("Port Open Success")
if dxl.setBaudRate(self.socket, self.baudRate):
print("Port Baud Set Success")
def __init__(self, protocol, devices, baudrate):
self.protocol = protocol
if isinstance(devices, list):
self.devices = devices
else:
# If it is not a list, make it one
self.devices = [devices]
self.baudrate = baudrate
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
self.port_num = []
for device in self.devices:
port = dynamixel.portHandler(device)
# remember port
self.port_num.append(port)
for i in range(0, len(self.devices)):
# open port
if dynamixel.openPort(self.port_num[i]):
print("Succeeded to open the port " + str(self.devices[i]))
else:
print("Failed to open the port " + str(self.devices[i]))
print("Press any key to terminate...")
getch()
quit()
# set baudrate
if dynamixel.setBaudRate(self.port_num[i], self.baudrate):
print("Succeeded to change the baudrate to " + str(self.baudrate))
else:
print("Failed to change the baudrate to " + str(self.baudrate))
print("Press any key to terminate...")
getch()
quit()
# Initialize PacketHandler Structs
dynamixel.packetHandler()
def __init__(self,device,baud):
self.fd = sys.stdin.fileno()
self.old_settings = termios.tcgetattr(self.fd)
self.port_num = dynamixel.portHandler(device)
dynamixel.packetHandler()
# 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...")
self.getch()
quit()
# Set port baudrate
if dynamixel.setBaudRate(self.port_num, baud):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
self.getch()
quit()
def initialize_port(self):
servo.port_num = dynamixel.portHandler(self.DEVICENAME)
dynamixel.packetHandler()
success_open_port = dynamixel.openPort(servo.port_num)
if servo.debug:
if success_open_port:
print("Succeeded to open the port!")
else:
print("Failed to open the port!")
input("Press any key to terminate...")
quit()
if success_open_port:
success_set_baudrate = dynamixel.setBaudRate(servo.port_num, self.BAUDRATE)
if servo.debug:
if success_set_baudrate:
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
input("Press any key to terminate...")
quit()
def initialize(self):
self.debug = True
## Constants / Parameters
self.wheel_left_ID = 28
self.wheel_right_ID = 29
self.pan_servo_ID = 20
self.tilt_servo_ID = 19
self.wheel_diameter = 52.0 / 1000 # meters
self.wheel_separation = 289.6 / 1000 # meters
self.WHEEL_MAX = 4096
self.SERVO_MAX = 1024
self.pan_lower_limit = 256
self.pan_upper_limit = 768
self.SERVO_CENTER = 512
self.MX_MAX_RPM = 117.07 # RPM
self.MX_MAX_VELOCITY = self.MX_MAX_RPM / 60.0 * 2 * PI # Radians / sec
## Global Variables:
self.x_pos = 0.0
self.y_pos = 0.0
self.theta = 0.0
self.prev_time = time.time()
self.pan_current_pos = 512
self.tilt_current_pos = 512
self.wheel_left_pos = 0
self.wheel_right_pos = 0
# Check which port is being used on your controller
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0"
self.DEVICENAME = "/dev/ttyUSB0".encode('utf-8')
## 1) Connect to Dynamixel USB
self.port_num = dyn.portHandler(self.DEVICENAME)
dyn.packetHandler()
if not dyn.openPort(self.port_num):
print("Failed to connect to Dynamixel port.")
exit(1)
time.sleep(1)
## 3) Read curren wheel positions
self.wheel_left_pos = dyn.get_position(self.port_num, self.wheel_left_ID)
self.wheel_right_pos = dyn.get_position(self.port_num, self.wheel_right_ID)
## 4) Reset Pan / Tilt to center
dyn.set_target_position(self.port_num, self.pan_servo_ID, self.SERVO_CENTER)
dyn.set_target_position(self.port_num, self.tilt_servo_ID, self.SERVO_CENTER)
#----------------------------
# Initialize subscribers
#----------------------------
self.create_subscriber("cmd-vel", self.twist_callback)
self.odom_pub = self.create_publisher('odometry')
#----------------------------
# Initialize subscribers
#----------------------------
self.rate = 1
self.poll_rate = 2
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_MINIMUM_POSITION_VALUE = -150000 # Dynamixel will rotate between this value
DXL_MAXIMUM_POSITION_VALUE = 150000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
DXL_MOVING_STATUS_THRESHOLD = 10 # Dynamixel moving status threshold
ESC_ASCII_VALUE = 0x1b
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_num1 = dynamixel.portHandler(DEVICENAME1)
port_num2 = dynamixel.portHandler(DEVICENAME2)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
index = 0
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE,
DXL_MAXIMUM_POSITION_VALUE] # Goal position
dxl_error = 0 # Dynamixel error
dxl1_present_position = 0 # Present position
dxl2_present_position = 0
# Open port1
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 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')
PRO_OPMODE_VEL = 1
PRO_OPMODE_POS = 3
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_PRO_MINIMUM_POSITION_VALUE = -150000 # Dynamixel will rotate between this value
DXL_PRO_MAXIMUM_POSITION_VALUE = 150000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
DXL_PRO_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold
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)
index = 0
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_error = 0 # Dynamixel error
dxl_present_position = 0 # Present position
def init():
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Open port
if dynamixel.openPort(port_num):
TORQUE_ENABLE = 1 # Value for enabling the torque TORQUE_DISABLE = 0 # Value for disabling the torque DXL_MINIMUM_POSITION_VALUE = -150000 # Dynamixel will rotate between this value DXL_MAXIMUM_POSITION_VALUE = 150000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) DXL_MOVING_STATUS_THRESHOLD = 10 # Dynamixel moving status threshold ESC_ASCII_VALUE = 0x1b 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_num1 = dynamixel.portHandler(DEVICENAME1) port_num2 = dynamixel.portHandler(DEVICENAME2) # Initialize PacketHandler Structs dynamixel.packetHandler() index = 0 dxl_comm_result = COMM_TX_FAIL # Communication result dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position dxl_error = 0 # Dynamixel error dxl1_present_position = 0 # Present position dxl2_present_position = 0 # Open port1 if dynamixel.openPort(port_num1):
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)
def search(id_search_min, id_search_max, baudrates_search_list):
"""Search for servos in range of ``id_search_min`` and ``id_search_max`` for all baudrates in ``baudrates_search_list``.
:param int id_search_min: ID to start searching.
:param int id_search_max: ID to stop pinging.
:param list baudrates_search_list: List containing the baudrates that the user want to search.
:return: ``found_servos`` list containing the servos found.
:rtype: List containing the ``Dynamixel`` object servos"""
#SEARCHING IN THE NETWORK
# 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
#Declaring the limits of the search
init = id_search_min
end = id_search_max
#List containing the found servos in the network
found_servos = []
#Tries to ping in protocols 1 and 2
for protocol in PROTOCOL_VERSIONS:
print("Using protocol %s" % str(protocol))
#Loop through all baudrates
for baudrate in baudrates_search_list:
actual_id = init
# 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
time.sleep(0.2)
while actual_id <= end:
print("Pinging in ID: %s " % actual_id)
# Try to ping the Dynamixel
# Get Dynamixel model number
dxl_model_number = dynamixel.pingGetModelNum(
port_num, protocol, actual_id)
dxl_comm_result = dynamixel.getLastTxRxResult(
port_num, protocol)
dxl_error = dynamixel.getLastRxPacketError(port_num, protocol)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(protocol, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(protocol, dxl_error))
else:
print(
"[ID:%03d] ping Succeeded. Dynamixel model number : %d"
% (actual_id, dxl_model_number))
servo = Dynamixel()
servo.id = actual_id
servo.baudrate = baudrate
servo.protocol = protocol
servo.model = dxl_model_number
found_servos.append(servo)
actual_id = actual_id + 1
# Close port
dynamixel.closePort(port_num)
return found_servos
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
DXL_ID = 1 # Dynamixel ID: 1
BAUDRATE = 57600
DEVICENAME = "/dev/ttyUSB0".encode("utf-8")# Check which port is being used on your controller
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
FACTORYRST_DEFAULTBAUDRATE = 57600 # Dynamixel baudrate set by factoryreset
NEW_BAUDNUM = 1 # New baudnum to recover Dynamixel baudrate as it was
OPERATION_MODE = 0x00 # Mode is unavailable in Protocol 1.0 Reset
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()
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_error = 0 # Dynamixel error
dxl_baudnum_read = 0 # Read baudnum
# 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...")
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)
