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 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 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 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 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 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 SetCCWAngleLimit(self, Id, angleLimit):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.CCW_ANGLE_LIMIT, angleLimit)
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 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 Init_Port_And_Motors(self):
# Open port
if dynamixel.openPort(self.port_num):
print("Successfully opened the port!")
else:
print("Failed to open the port!")
print("Press any key to terminate...")
getch()
quit()
# Set port baudrate
if dynamixel.setBaudRate(self.port_num, self.baud_rate):
print("Successfully set the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
ADDR_PRO_TORQUE_ENABLE = 64
TORQUE_ENABLE = 1
dxl_comm_result = COMM_TX_FAIL
# Enable Dynamixel#1 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m1id, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num, self.proto_ver)
dxl_error = dynamixel.getLastRxPacketError(self.port_num, self.proto_ver)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(self.proto_ver, dxl_error))
else:
print("Dynamixel#1 has been successfully connected")
# Enable Dynamixel#2 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m2id, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num, self.proto_ver)
dxl_error = dynamixel.getLastRxPacketError(self.port_num, self.proto_ver)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(self.proto_ver, dxl_error))
else:
print("Dynamixel#2 has been successfully connected")
# Enable Dynamixel#3 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m3id, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num, self.proto_ver)
dxl_error = dynamixel.getLastRxPacketError(self.port_num, self.proto_ver)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(self.proto_ver, dxl_error))
else:
print("Dynamixel#3 has been successfully connected")
# Enable Dynamixel#4 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m4id, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num, self.proto_ver)
dxl_error = dynamixel.getLastRxPacketError(self.port_num, self.proto_ver)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(self.proto_ver, dxl_error))
else:
print("Dynamixel#4 has been successfully connected")
def SetTorqueLimit(self, Id, torqueLimit):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.TORQUE_LIMIT, torqueLimit)
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 torque limit..."
def set_compliance(self,
compliance_margin,
compliance_slope,
num_tries=RETRIES):
retry = 0
while (retry < num_tries):
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CW_COMPLIANCE_M, compliance_margin)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CCW_COMPLIANCE_M, compliance_margin)
dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL)
derror += dxl.getLastRxPacketError(self.port, PROTOCOL)
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CW_COMPLIANCE_S, compliance_slope)
dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL)
derror += dxl.getLastRxPacketError(self.port, PROTOCOL)
dxl.write1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CCW_COMPLIANCE_S, compliance_slope)
dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL)
derror += dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
return 0
raise Exception("set_gains failed after {} tries".format(num_tries))
def 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 get_mode(self, num_tries=RETRIES):
mode = 0
retry = 0
while (retry < num_tries):
cw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CW_LIMIT)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
ccw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CCW_LIMIT)
dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
if ((cw_lim == 0) & (ccw_lim == 0)):
mode = 1
else:
mode = 2
return mode
print('[COMM_ERR] ' + str(get_caller()))
return 0
def SetMaxTorque(self, Id, maxTorque):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MAX_TORQUE, maxTorque)
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 max torque..."
def get_mode(self, num_tries=RETRIES):
mode = 0
retry = 0
while (retry < num_tries):
cw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CW_LIMIT)
dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
ccw_lim = dxl.read1ByteTxRx(self.port, PROTOCOL, self.id,
ADDR_CCW_LIMIT)
dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL)
derror = dxl.getLastRxPacketError(self.port, PROTOCOL)
if dcomm_result != 0:
print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result))
retry += 1
elif derror != 0:
print_v(dxl.getRxPacketError(PROTOCOL, derror))
retry += 1
else:
if ((cw_lim == 0) & (ccw_lim == 0)):
mode = 1
else:
mode = 2
return mode
raise Exception("get_mode failed after {} tries".format(num_tries))
def Disable_Torque_Close_Port(self):
ADDR_PRO_TORQUE_ENABLE = 64
TORQUE_DISABLE = 0
dxl_comm_result = COMM_TX_FAIL
# Enable Dynamixel#1 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m1id,
ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num,
self.proto_ver)
dxl_error = dynamixel.getLastRxPacketError(self.port_num,
self.proto_ver)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(self.proto_ver, dxl_error))
else:
print("Dynamixel#1 has been successfully freed")
# Enable Dynamixel#2 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m2id,
ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num,
self.proto_ver)
dxl_error = dynamixel.getLastRxPacketError(self.port_num,
self.proto_ver)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(self.proto_ver, dxl_error))
else:
print("Dynamixel#2 has been successfully freed")
# Enable Dynamixel#3 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m3id,
ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num,
self.proto_ver)
dxl_error = dynamixel.getLastRxPacketError(self.port_num,
self.proto_ver)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(self.proto_ver, dxl_error))
else:
print("Dynamixel#3 has been successfully freed")
# Enable Dynamixel#4 Torque
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m4id,
ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num,
self.proto_ver)
dxl_error = dynamixel.getLastRxPacketError(self.port_num,
self.proto_ver)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(self.proto_ver, dxl_error))
else:
print("Dynamixel#4 has been successfully freed")
#close port
dynamixel.closePort(self.port_num)
def GetPresentLoad(self, Id):
try:
present_load = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.PRESENT_LOAD)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return present_load
except :
print "Oops! Problem writing goal position..."
def GetPresentVoltage(self, Id):
try:
present_voltaje = dynamixel.read1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.PRESENT_VOLTAGE)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return present_voltaje
except :
print "Oops! Problem writing goal position..."
def GetTorqueLimit(self, Id):
try:
torque_limit = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.TORQUE_LIMIT)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return torque_limit
except :
print "Oops! Problem reading Torque limit..."
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 GetMovingSpeed(self, Id):
try:
moving_speed = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MOVING_SPEED)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return moving_speed
except :
print "Oops! Problem reading moving speed..."
def GetGoalPosition(self, Id):
try:
goal_position = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.GOAL_POSITION)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return goal_position
except :
print "Oops! Problem reading..."
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 GetMaxTorque(self, Id, maxTorque):
try:
max_torque = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MAX_TORQUE)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return max_torque
except :
print "Oops! Problem reading max torque..."
def GetCCWAngleLimit(self, Id):
try:
ccw_angle_limit = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.CCW_ANGLE_LIMIT)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return ccw_angle_limit
except :
print "Oops! Problem reading..."
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 SetCCWAngleLimit(self, Id, angleLimit):
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.CCW_ANGLE_LIMIT, angleLimit)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
def GetTorqueEnable(self, Id):
try:
torque_enable = dynamixel.read1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.TORQUE_ENABLE)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return torque_enable
except :
print "Oops! Problem reading Torque..."
return -3141592
def SetCCWComplianceMargin(self, Id, ComMargCCW):
dynamixel.write1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.CCW_COMPLIANCE_MARGIN, ComMargCCW)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
def SetCCWComplianceSlope(self, Id, ComSlopeCCW):
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))
def SetMaxTorque(self, Id, maxTorque):
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.MAX_TORQUE, maxTorque)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
def SetMovingSpeed(self, Id, movingSpeed):
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.MOVING_SPEED, movingSpeed)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
def SetAlarmShutdown(self, Id, alarmShutdown):
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))
def SetGoalPosition(self, Id, goalPose):
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.GOAL_POSITION, goalPose)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
def SetPGain(self, Id, PGain):
dynamixel.write1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.CW_COMPLIANCE_SLOPE, PGain)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
def SetTorqueLimit(self, Id, torqueLimit):
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.TORQUE_LIMIT, torqueLimit)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
def SetHighestLimitTemperature(self, Id, highestLimitTemp):
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))
def SetMovingSpeed(self, Id, movingSpeed):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MOVING_SPEED, movingSpeed)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return False
else:
return True
except :
print "Oops! Problem writing moving speed..."
def GetPresentPosition(self, Id):
try:
present_position = dynamixel.read2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.PRESENT_POSITION)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
else:
return present_position
except :
print "Oops! Problem writing goal position..."
return -3141592
def GetTorqueEnable(self, Id):
torque_enable = dynamixel.read1ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION, Id,
Registers.TORQUE_ENABLE)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return torque_enable
def GetMovingSpeed(self, Id):
moving_speed = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION, Id,
Registers.MOVING_SPEED)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return moving_speed
def SetTorqueVale(self, Id, torqueValue, directionTurn):
if directionTurn == True:
torqueValue = torqueValue + 1024
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id,
Registers.MOVING_SPEED, movingSpeed)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
def GetMaxTorque(self, Id, maxTorque):
max_torque = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION, Id,
Registers.MAX_TORQUE)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return max_torque
def GetTorqueLimit(self, Id):
torque_limit = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION, Id,
Registers.TORQUE_LIMIT)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return torque_limit
def GetPresentLoad(self, Id):
present_load = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION, Id,
Registers.PRESENT_LOAD)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return present_load
def GetGoalPosition(self, Id):
goal_position = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION, Id,
Registers.GOAL_POSITION)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return goal_position
def GetCWAngleLimit(self, Id):
cw_angle_limit = dynamixel.read2ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION,
Id, Registers.CW_ANGLE_LIMIT)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return cw_angle_limit
def SetTorqueVale(self, Id, torqueValue, directionTurn):
try:
if directionTurn == True:
torqueValue = torqueValue + 1024
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.MOVING_SPEED, torqueValue)
if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
return False
else:
return True
except :
print "Oops! Problem writing torque value..."
def GetPresentVoltage(self, Id):
present_voltaje = dynamixel.read1ByteTxRx(self.port_num,
Registers.PROTOCOL_VERSION,
Id,
Registers.PRESENT_VOLTAGE)
if dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(
Registers.PROTOCOL_VERSION,
dynamixel.getLastRxPacketError(self.port_num,
Registers.PROTOCOL_VERSION))
return present_voltaje
def SetGoalPosition(self, Id, goalPose):
try:
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.GOAL_POSITION, goalPose)
dynamixel.write2ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.GOAL_POSITION, goalPose)
if dynamixel.getLastTxRxResult(self.port_num, Registers.PROTOCOL_VERSION) != Registers.COMM_SUCCESS:
#dynamixel.printTxRxResult(Registers.PROTOCOL_VERSION, dynamixel.getLastTxRxResult(self.port_num, Registers.PROTOCOL_VERSION))
return False
elif dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION))
else:
return True
except :
print "Oops! Problem writing..."
return False
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
# Read present baudrate of the controller
print("Now the controller baudrate is : %d" % (dynamixel.getBaudRate(port_num)))
# Try factoryreset
print("[ID:%03d] Try factoryreset : " % (DXL_ID))
dynamixel.factoryReset(port_num, PROTOCOL_VERSION, DXL_ID, OPERATION_MODE)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print("Aborted")
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
# Wait for reset
print("Wait for reset...")
sleep(2)
print("[ID:%03d] factoryReset Success!" % (DXL_ID))
# Set controller baudrate to dxl default baudrate
if dynamixel.setBaudRate(port_num, FACTORYRST_DEFAULTBAUDRATE):
# 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#1 Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_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#1 has been successfully connected")
# Enable Dynamixel#2 Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_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#2 has been successfully connected")
while 1: