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