def setAngle(na):
print('setAngle begins')
global targetAngle
while True:
newTargetAngle = L.getTargets()[1]
#print('targetAngle, newTA: ' + str(targetAngle) + str(newTargetAngle))
#if targetAngle changes, send new angle to dynamixel
if targetAngle != newTargetAngle:
motorValue = mapTargetAngleToMotorValue(newTargetAngle)
targetAngle = newTargetAngle
#print(' targetAngle', targetAngle)
#motorValue = motorValue - 109 ????
#print('newMotorvalue' , motorValue) ???
if motorValue <= 100 or motorValue >= 4000:
return
# Write goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_MOVING_SPEED, 200)
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_GOAL_POSITION, motorValue)
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 setAngle(self, steerAngle):
print('setAngle begins')
self.mapTargetAngleToMotorValue(steerAngle)
print("Angle: " + str(steerAngle) + " mappedValue: " +
str(self.mappedSteerAngle))
if self.mappedSteerAngle <= 100 or self.mappedSteerAngle >= 4000:
return
# Write goal position
dynamixel.write2ByteTxRx(self.port_num, self.PROTOCOL_VERSION,
self.DXL_ID, self.ADDR_MX_MOVING_SPEED, 200)
dynamixel.write2ByteTxRx(self.port_num, self.PROTOCOL_VERSION,
self.DXL_ID, self.ADDR_MX_GOAL_POSITION,
int(self.mappedSteerAngle))
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))
print('setAngle finished')
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 dxl_torcon(goal_torq):
# Write goal position
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, 1, ADDR_MX_GOAL_TORQUE, goal_torq[0])
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 torque_on(): # Add Dynamixels torque mode on to the Syncwrite storage for i in xrange(6): dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, i+1, ADDR_MX_TORQUE_ON, 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 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 CheckTxRxResult(self):
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))
return False
elif dynamixel.getLastRxPacketError(self.port_num,
self.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
self.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
self.PROTOCOL_VERSION))
return False
return True
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)
#Writing the value in the address
dynamixel.write2ByteTxRx(port_num, PROTOCOL_1 , id, ADDR_MAX_TORQUE, value)
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:
time.sleep(0.2)
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)
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("Baudrate changed to : %s" % baudrate)
time.sleep(0.2)
def dxl_torqcon(goal_torq):
# Add Dynamixels goal position value to the Syncwrite storage
for i in xrange(6):#
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(group_num, i+1, goal_torq[i], LEN_MX_GOAL_TORQUE)).value
print(dxl_addparam_result)
if dxl_addparam_result != 1:
print(dxl_addparam_result)
print("[ID:%03d] groupSyncWrite addparam failed" % (i+1))
quit()
# Syncwrite goal position
dynamixel.groupSyncWriteTxPacket(group_num)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
# Clear syncwrite parameter storage
dynamixel.groupSyncWriteClearParam(group_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 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)
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("ID changed")
time.sleep(0.2)
def dxl_init():
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
# 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()
# Enable Dynamixel Torque
for i in xrange(6):
while 1:
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, i + 1,
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 %d has been successfully connected" % (i + 1))
break
# Open port
if dynamixel.openPort(port_num):
print("Succeeded to open the port!")
else:
print("Failed to open the port!")
print("Press any key to terminate...")
getch()
quit()
# Set port baudrate
if dynamixel.setBaudRate(port_num, BAUDRATE):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
# Try to broadcast ping the Dynamixel
dynamixel.broadcastPing(port_num, PROTOCOL_VERSION)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
print("Detected Dynamixel : ")
for id in range(0, MAX_ID):
if ctypes.c_ubyte(dynamixel.getBroadcastPingResult(port_num, PROTOCOL_VERSION, id)).value:
print("[ID:%03d]" % (id))
# Close port
dynamixel.closePort(port_num)
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
# Read present baudrate of the controller
print("Now the controller baudrate is : %d" % (dynamixel.getBaudRate(port_num)))
# Try factoryreset
print("[ID:%03d] Try factoryreset : " % (DXL_ID))
dynamixel.factoryReset(port_num, PROTOCOL_VERSION, DXL_ID, OPERATION_MODE)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
print("Aborted")
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
quit()
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
# Wait for reset
print("Wait for reset...")
sleep(2)
print("[ID:%03d] factoryReset Success!" % (DXL_ID))
# Set controller baudrate to dxl default baudrate
if dynamixel.setBaudRate(port_num, FACTORYRST_DEFAULTBAUDRATE):
print("Succeed to change the controller baudrate to : %d" % (FACTORYRST_DEFAULTBAUDRATE))
else:
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 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 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)
if dynamixel.getLastTxRxResult(port_num, protocol) != COMM_SUCCESS:
dynamixel.printTxRxResult(protocol, dynamixel.getLastTxRxResult(port_num, protocol))
elif dynamixel.getLastRxPacketError(port_num, protocol) != 0:
dynamixel.printRxPacketError(protocol, dynamixel.getLastRxPacketError(port_num, protocol))
else:
#Case the ping succeeds, creates an servo object and stores it in the found_servos vector
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 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)
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:
#D gain set
time.sleep(0.5)
# I gain config
dynamixel.write1ByteTxRx(port_num, PROTOCOL_1, id, ADDR_I_GAIN, i_gain)
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:
#I gain set
time.sleep(0.5)
# P gain config
dynamixel.write1ByteTxRx(port_num, PROTOCOL_1, id, ADDR_P_GAIN, p_gain)
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:
#P gain set
time.sleep(0.5)
print("Gain changed")
