def get_present_position(self, ids):
positions = []
for id in ids:
present_position = dxl.read4ByteTxRx(self.port, self.protocol, id,
ADDR_PRES_POS)
self.check_result(id)
self.check_error(id)
present_position = self.to_degree(present_position) - 180
positions.append(present_position)
return positions
def read_4(self, id, reg, port=0, doPrint=False):
read_res = dynamixel.read4ByteTxRx(self.port_num[port], self.protocol, id, reg)
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))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(self.protocol, dxl_error))
if doPrint:
print("[ID:%03d] Regist %03d: %03d" % (id, reg, read_res))
return read_res
def read_present_position(self):
dxl_present_position = dynamixel.read4ByteTxRx(servo.port_num, servo.PROTOCOL_VERSION, self.ID,
servo.ADDR_PRO_PRESENT_POSITION)
dxl_comm_result = dynamixel.getLastTxRxResult(servo.port_num, servo.PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(servo.port_num, servo.PROTOCOL_VERSION)
if dxl_comm_result == servo.COMM_SUCCESS:
return dxl_present_position
else:
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))
return servo.read_present_position(self)
def Read_Value(self, motorId, read_addr, read_len):
# Read value
dxl_result = -1
if(read_len == 4):
dxl_result = dynamixel.read4ByteTxRx(self.port_num, self.proto_ver, motorId, read_addr)
elif(read_len == 2):
dxl_result = dynamixel.read2ByteTxRx(self.port_num, self.proto_ver, motorId, read_addr)
elif(read_len == 1):
dxl_result = dynamixel.read1ByteTxRx(self.port_num, self.proto_ver, motorId, read_addr)
else:
print('[ID:%03d]: invalid read length %d' %(motorId, read_len))
return dxl_result
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))
return dxl_result
def pwm_control(dxl_id, goal_pos):
# Read present position
dxl_present_position = dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION,
dxl_id,
ADDR_X_PRESENT_POSITION)
goalpwm = (goal_pos - dxl_present_position) * 10
goalpwm = PWM_LIMIT if goalpwm > PWM_LIMIT else -1 * PWM_LIMIT if goalpwm < -1 * PWM_LIMIT else goalpwm
#goalpwm = -1 * goalpwm + 2048 if goalpwm < 0 else goalpwm
#print "goalpwm", goalpwm
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, dxl_id,
ADDR_X_GOAL_PWM, goalpwm)
#print("[ID:%03d] GoalPos:%03d PresPos:%03d" % (dxl_id, dxl_goal_position[index[dxl_id]], dxl_present_position))
if not (abs(goal_pos - dxl_present_position) >
DXL_MOVING_STATUS_THRESHOLD):
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, dxl_id,
ADDR_X_GOAL_PWM, 0)
return True
return False
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))
while 1:
# Read present position
dxl1_present_position = dynamixel.read4ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_PRESENT_POSITION)
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))
# Read present position
dxl2_present_position = dynamixel.read4ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_PRESENT_POSITION)
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))
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))
while 1:
# Read present position
dxl1_present_position = dynamixel.read4ByteTxRx(
port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_PRESENT_POSITION)
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))
# Read present position
dxl2_present_position = dynamixel.read4ByteTxRx(
port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_PRESENT_POSITION)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num2,
PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(ESC_ASCII_VALUE):
break
# Write goal position
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index])
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))
while 1:
# Read present position
dxl_present_position = dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_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))
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
def get_moving_speed(self, ids):
moving_speed = {}
for i in ids:
dxl_present_speed = dynamixel.read4ByteTxRx(self.portHandler, self.PROTOCOL_VERSION, i, self.ADDR_MX_PRESENT_SPEED)
moving_speed[i] = dxl_present_speed
return moving_speed
if getch() == chr(ESC_ASCII_VALUE):
break
# Write goal position
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID,
ADDR_PRO_GOAL_POSITION, dxl_goal_position[index])
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))
while 1:
# Read present position
dxl_present_position = dynamixel.read4ByteTxRx(
port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_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))
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
def Position(self):
position = dynamixel.read4ByteTxRx(self.port_num,
self.PROTOCOL_VERSION, self.DXL_ID,
self.ADDR_PRO_PRESENT_POSITION)
if not self.CheckTxRxResult(): return None
return position
