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)