def _write_speed(self, DXL_ID, speed):
dynamixel.write4ByteTxRx(self.portHandler, self.PROTOCOL_VERSION, DXL_ID, 44, 1023)
dynamixel.write4ByteTxRx(self.portHandler, self.PROTOCOL_VERSION, DXL_ID, self.ADDR_MX_SPEED, int(speed))
dxl_comm_result= dynamixel.getLastTxRxResult(self.portHandler, self.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(self.portHandler, self.PROTOCOL_VERSION)
if(dxl_comm_result != COMM_SUCCESS):
print(DXL_ID,dynamixel.getTxRxResult(self.PROTOCOL_VERSION, self.dxl_comm_result))
elif(dxl_error != 0):
print(DXL_ID,dynamixel.getRxPacketError(self.PROTOCOL_VERSION, dxl_error))
def control():
actual_vel = 0
upgoing = True
rospy.init_node('dynamixelpronode', anonymous=True)
#rospy.Subscriber("/dyn_ef_robot/dyn_ef_robot_controller/follow_joint_trajectory/goal", String, setNewTrajectory)
rate = rospy.Rate(20) # 10hz
while not rospy.is_shutdown():
# print("Press any key to continue! (or press ESC to quit!)")
# if getch() == chr(ESC_ASCII_VALUE):
# break
# print("hallo")
if upgoing:
actual_vel += 100
if actual_vel >= 4000:
upgoing = False
else:
actual_vel -= 100
if actual_vel <= -4000:
upgoing = True
# Write goal position
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_PRO_ID, ADDR_PRO_GOAL_VELOCITY, actual_vel)
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_106_ID, ADDR_106_GOAL_VELOCITY, actual_vel/16)
# if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
# print("not successful")
# 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))
# while 1:
# # Read present position
# print("read present position")
# dxl_present_position = dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_PRESENT_POSITION)
# 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))
# print("[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position))
# if not (abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD):
# break
# Change goal position
# if index == 0:
# index = 1
# else:
# index = 0
rate.sleep()
# Disable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_PRO_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_106_ID, ADDR_106_TORQUE_ENABLE, TORQUE_DISABLE)
def write_velocity(self, dxl_goal_velocity):
dynamixel.write4ByteTxRx(servo.port_num, servo.PROTOCOL_VERSION, self.ID, servo.ADDR_PRO_GOAL_VELOCITY,
dxl_goal_velocity)
dxl_comm_result = dynamixel.getLastTxRxResult(servo.port_num, servo.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(servo.port_num, servo.PROTOCOL_VERSION)
if servo.debug:
if dxl_comm_result != servo.COMM_SUCCESS:
print(dynamixel.getTxRxResult(servo.PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(servo.PROTOCOL_VERSION, dxl_error))
def write_4(self, id, reg, value, port=0, doPrint=False):
dynamixel.write4ByteTxRx(self.port_num[port], self.protocol, id, reg, value)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num[port], self.protocol)
dxl_error = dynamixel.getLastRxPacketError(self.port_num[port], self.protocol)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.protocol, dxl_comm_result))
return False
elif dxl_error != 0:
print(dynamixel.getRxPacketError(self.protocol, dxl_error))
return False
return True
def write_position(self, dxl_goal_position):
if dxl_goal_position <= self.POS_MAX and dxl_goal_position >= self.POS_MIN:
dynamixel.write4ByteTxRx(servo.port_num, servo.PROTOCOL_VERSION, self.ID, servo.ADDR_PRO_GOAL_POSITION,
dxl_goal_position)
dxl_comm_result = dynamixel.getLastTxRxResult(servo.port_num, servo.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(servo.port_num, servo.PROTOCOL_VERSION)
if servo.debug:
if dxl_comm_result != servo.COMM_SUCCESS:
print(dynamixel.getTxRxResult(servo.PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(servo.PROTOCOL_VERSION, dxl_error))
elif servo.debug:
print("Goalposition of Servo ", self.ID, " out of range!")
def Set_Value(self, motorId, set_addr, set_len, value):
if(set_len == 4):
dynamixel.write4ByteTxRx(self.port_num, self.proto_ver, motorId, set_addr, value)
elif(set_len == 2):
dynamixel.write2ByteTxRx(self.port_num, self.proto_ver, motorId, set_addr, value)
elif(set_len == 1):
dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, motorId, set_addr, value)
else:
print('[ID:%03d]: invalid set length %d' %(motorId, set_len))
return
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))
def MoveTo(self, target, wait=True, threshold=20):
target = int(target)
if target < self.DXL_MINIMUM_POSITION_VALUE:
target = self.DXL_MINIMUM_POSITION_VALUE
elif target > self.DXL_MAXIMUM_POSITION_VALUE:
target = self.DXL_MAXIMUM_POSITION_VALUE
# Write goal position
dynamixel.write4ByteTxRx(self.port_num, self.PROTOCOL_VERSION,
self.DXL_ID, self.ADDR_PRO_GOAL_POSITION,
target)
if not self.CheckTxRxResult(): return
while wait:
pos = self.Position()
if pos is None: return
#print("[ID:%03d] GoalPos:%03d PresPos:%03d" % (self.DXL_ID, target, pos))
if not (abs(target - pos) > threshold): break
def initDxl106():
# set opmode to velocity control
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_106_ID, ADDR_106_OP_MODE, PRO_OPMODE_VEL)
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_106_ID, ADDR_106_MAXPOS_LIMIT, 4090)
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_106_ID, ADDR_106_MINPOS_LIMIT, 10)
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_106_ID, ADDR_106_VEL_LIMIT, 500)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_106_ID, ADDR_106_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
else:
print("Dynamixel#2 has been successfully connected")
while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(ESC_ASCII_VALUE):
break
# Write Dynamixel#1 goal position
dynamixel.write4ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index])
dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
# Write Dynamixel#2 goal position
dynamixel.write4ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index])
dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
else:
print("Dynamixel#2 has been successfully connected")
while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(ESC_ASCII_VALUE):
break
# Write Dynamixel#1 goal position
dynamixel.write4ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID,
ADDR_PRO_GOAL_POSITION, dxl_goal_position[index])
dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
# Write Dynamixel#2 goal position
dynamixel.write4ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID,
ADDR_PRO_GOAL_POSITION, dxl_goal_position[index])
dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
# 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()
# Acceleration Limit #1
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_XM430_ACCELERATION_LIMIT, dxl_acc_limit)
# Acceleration Limit #2
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_XM430_ACCELERATION_LIMIT, dxl_acc_limit)
# Acceleration Limit #3
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL3_ID, ADDR_XM430_ACCELERATION_LIMIT, dxl_acc_limit)
# Acceleration Limit #4
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL4_ID, ADDR_XM430_ACCELERATION_LIMIT, dxl_acc_limit)
# Acceleration Limit #5
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL5_ID, ADDR_XM430_ACCELERATION_LIMIT, dxl_acc_limit)
# Acceleration Limit #6
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL6_ID, ADDR_XM430_ACCELERATION_LIMIT, dxl_acc_limit)
# Acceleration Limit #7
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL7_ID, ADDR_XM430_ACCELERATION_LIMIT, dxl_acc_limit)
# Velocity Limit #1
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit)
def set_acc_profile(self, ids, value):
for id in ids:
dxl.write4ByteTxRx(self.port, self.protocol, id, ADDR_PROF_ACC,
value)
self.check_result(id)
self.check_error(id)
def set_profile_velocity(self, ids, value):
for id in ids:
dxl.write4ByteTxRx(self.port, self.protocol, id, ADDR_PROF_VEL,
value)
self.check_result(id)
self.check_error(id)
break
# Write goal position
check_errors()
start_time = time.time()
for i in range(steps_before_key):
for dxl_id in DXL_IDS:
goal_pos = index[dxl_id]
if pwm_control(dxl_id, dxl_goal_position[goal_pos]):
index[dxl_id] = 0 if index[dxl_id] == 1 else 1
check_errors()
duration = time.time() - start_time
print "duration: %.2fs" % (duration)
print "%.2fHz" % (steps_before_key / duration)
for dxl_id in DXL_IDS:
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, dxl_id,
ADDR_X_GOAL_PWM, 0)
# for dxl_id in DXL_IDS:
#
# print index
for dxl_id in DXL_IDS:
disable_torque(dxl_id)
check_errors()
# Close port
dynamixel.closePort(port_num)
