Ejemplo n.º 1
0
    def _syncWrite(self, servos, addr, info_len, values=None):

        ''' this is an adaptation from dynamixel's sdk for
            the sync_write '''
        SW = dxl.groupSyncWrite(self.socket, PROTOCOL_VERSION, addr, info_len)
        for i, s in enumerate(servos):
            if(values is None):
                dxl.groupSyncWriteAddParam(SW, s.servo_id, s.goalValue, info_len)
            else:
                dxl.groupSyncWriteAddParam(SW, s.servo_id, values[i], info_len)

        dxl.groupSyncWriteTxPacket(SW) #does the sync write
        dxl.groupSyncWriteClearParam(SW) #clears buffer
Ejemplo n.º 2
0
    def _syncWrite(self, servos, addr, info_len, values=None):
        ''' this is an adaptation from dynamixel's sdk for
            the sync_write '''
        SW = dxl.groupSyncWrite(self.socket, PROTOCOL_VERSION, addr, info_len)
        for i, s in enumerate(servos):
            if (values is None):
                dxl.groupSyncWriteAddParam(SW, s.servo_id, s.goalValue,
                                           info_len)
            else:
                dxl.groupSyncWriteAddParam(SW, s.servo_id, values[i], info_len)

        dxl.groupSyncWriteTxPacket(SW)  #does the sync write
        dxl.groupSyncWriteClearParam(SW)  #clears buffer
Ejemplo n.º 3
0
def set_target_positions(pos_list):
    global times

    if (len(pos_list) != len(IDS)):
        print("pos_list doesn't have the same dimension as ids_list")
        return -1

    for i in range(len(pos_list)):
        # write goal position

        dxl_addparam_result = dynamixel.groupSyncWriteAddParam(
            GROUP_NUM, IDS[i], pos_list[i] + HOME, LEN_MX_GOAL_POSITION)
        if dxl_addparam_result != 1:
            print(dxl_addparam_result)
        """dynamixel.write2ByteTxRx(PORT_NUM, PROTOCOL_VERSION, IDS[i], ADDR_MX_GOAL_POSITION, pos_list[i] + HOME)
        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))
            return -1
        elif dxl_error != 0:
            print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
            return -1"""

    tv_gswtp = time()
    dynamixel.groupSyncWriteTxPacket(GROUP_NUM)
    times = helpers.dict_timer("DT.groupSyncWriteTxPacket", times,
                               time() - tv_gswtp)
    #    if dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION) != COMM_SUCCESS:
    #        dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION))
    tv_gswcp = time()
    dynamixel.groupSyncWriteClearParam(GROUP_NUM)
    times = helpers.dict_timer("DT.groupSyncWriteClearParam", times,
                               time() - tv_gswcp)
Ejemplo n.º 4
0
def set_target_positions(pos_list):

    if (len(pos_list) != len(IDS)):
        print("pos_list doesn't have the same dimension as ids_list")
        return -1

    for i in range(len(pos_list)):
        # write goal position

        dxl_addparam_result = ctypes.c_ubyte(
            dynamixel.groupSyncWriteAddParam(GROUP_NUM, IDS[i],
                                             pos_list[i] + HOME,
                                             LEN_MX_GOAL_POSITION)).value
        if dxl_addparam_result != 1:
            print(dx1_addparam_result)
        """dynamixel.write2ByteTxRx(PORT_NUM, PROTOCOL_VERSION, IDS[i], ADDR_MX_GOAL_POSITION, pos_list[i] + HOME)
        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))
            return -1
        elif dxl_error != 0:
            print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
            return -1"""

    dynamixel.groupSyncWriteTxPacket(GROUP_NUM)
    """if dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION) != COMM_SUCCESS:
        dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION))"""

    dynamixel.groupSyncWriteClearParam(GROUP_NUM)
Ejemplo n.º 5
0
 def set_goal_position(self, ids):
     # for i, angle in ids.iteritems():
         # self._write(i,angle)
     for i, angle in ids.iteritems():
             dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(self.groupSync, i, 2048 + int(angle / 0.088), self.LEN_MX_GOAL_POSITION)).value
     dynamixel.groupSyncWriteTxPacket(self.groupSync)
     dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler, self.PROTOCOL_VERSION)
     if(dxl_comm_result != COMM_SUCCESS):
         print(dynamixel.getTxRxResult(self.PROTOCOL_VERSION, self.dxl_comm_result))
     dynamixel.groupSyncWriteClearParam(self.groupSync)
Ejemplo n.º 6
0
 def group_sync_write_goal(self, commands):
     sync_group_num = dynamixel.groupSyncWrite(self.__port_num, PROTOCOL, GOAL_POSITION, 2)
     for servo_id, goal_position in commands:
         success = dynamixel.groupSyncWriteAddParam(sync_group_num, servo_id, int(goal_position), 2)
         if not success:
             raise IOError("failed writing to group sync write")
     dynamixel.groupSyncWriteTxPacket(sync_group_num)
     dxl_comm_result = dynamixel.getLastTxRxResult(self.__port_num, PROTOCOL)
     if dxl_comm_result != 0:
         error_desciption = get_tx_rx_result_description(dxl_comm_result)
         raise IOError("dxl_comm_result: " + error_desciption)
     dynamixel.groupSyncWriteClearParam(sync_group_num)
Ejemplo n.º 7
0
def dxl_torqcon(goal_torq):
	# Add Dynamixels goal position value to the Syncwrite storage
	for i in xrange(6):#
		dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(group_num, i+1, goal_torq[i], LEN_MX_GOAL_TORQUE)).value
		print(dxl_addparam_result)
		if dxl_addparam_result != 1:
		    print(dxl_addparam_result)
		    print("[ID:%03d] groupSyncWrite addparam failed" % (i+1))
		    quit()

    # Syncwrite goal position
	dynamixel.groupSyncWriteTxPacket(group_num)
	if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
		dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))

	# Clear syncwrite parameter storage
	dynamixel.groupSyncWriteClearParam(group_num)
Ejemplo n.º 8
0
    def set_goal_position(self, write_dict):
        #self.set_torque_status(write_dict.keys(),1)
        for id, angle in write_dict.items():
            angle = self.from_degree(angle + 180)
            # Add dxl goal position value to the Syncwrite storage
            addparam_result = ctypes.c_ubyte(
                dxl.groupSyncWriteAddParam(self.groupwrite, id, angle,
                                           LEN_GOAL_POSITION)).value
            if addparam_result != 1:
                print("[ID:%03d] groupSyncWrite addparam failed" % (id))
                quit()

        # Syncwrite goal position
        dxl.groupSyncWriteTxPacket(self.groupwrite)
        self.check_result(id)

        # Clear syncwrite parameter storage
        dxl.groupSyncWriteClearParam(self.groupwrite)
        self.check_result(id)
Ejemplo n.º 9
0
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:
    print("Press any key to continue! (or press ESC to quit!)")
    if getch() == chr(ESC_ASCII_VALUE):
        break

    # Add Dynamixel#1 goal position value to the Syncwrite storage
    dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(group_num, DXL1_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION)).value
    print(dxl_addparam_result)
    if dxl_addparam_result != 1:
        print(dxl_addparam_result)
        print("[ID:%03d] groupSyncWrite addparam failed" % (DXL1_ID))
        quit()

    # Add Dynamixel#2 goal position value to the Syncwrite parameter storage
    dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(group_num, DXL2_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION)).value
    if dxl_addparam_result != 1:
        print("[ID:%03d] groupSyncWrite addparam failed" % (DXL2_ID))

    # Syncwrite goal position
    dynamixel.groupSyncWriteTxPacket(group_num)
    if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
        dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
Ejemplo n.º 10
0
            ## Se calcula el ángulo para el número de servomotor en el tiempo determinado
            angulo = Choset(amplitud_par, amplitud_impar, desfase, dtheta_dn,
                            dtheta_dt, (muestra + (10 * (t_time))), i,
                            offset_par, offset_impar)

            ## Si el módulo se seleccionó como inactivo, el ángulo se vuelve cero.
            angulo *= MOD_ACTIVOS[i]
            goalpos = 512 - angulo * 3.41
            print(goalpos)

            ## Se arma el paquete a enviar al servomotor. Es el envío del comando de posición
            ## Add Dynamixel#1 goal position value to the Syncwrite storage
            dxl_addparam_result = ctypes.c_ubyte(
                dynamixel.groupSyncWriteAddParam(group_num, (i + 1),
                                                 int(goalpos),
                                                 GOAL_POSITION_LEN)).value
            print(dxl_addparam_result)
            if dxl_addparam_result != 1:
                print(dxl_addparam_result)
                print("[ID:%03d] groupSyncWrite addparam failed" % (i + 1))
                quit()

            ## Syncwrite goal position
            dynamixel.groupSyncWriteTxPacket(group_num)
            if dynamixel.getLastTxRxResult(port_num,
                                           PROTOCOL_VERSION) != COMM_SUCCESS:
                dynamixel.printTxRxResult(
                    PROTOCOL_VERSION,
                    dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
Ejemplo n.º 11
0
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

    # Add Dynamixel#1 goal position value to the Syncwrite storage
    dxl_addparam_result = ctypes.c_ubyte(
        dynamixel.groupSyncWriteAddParam(group_num, DXL1_ID,
                                         dxl_goal_position[index],
                                         LEN_MX_GOAL_POSITION)).value
    print(dxl_addparam_result)
    if dxl_addparam_result != 1:
        print(dxl_addparam_result)
        print("[ID:%03d] groupSyncWrite addparam failed" % (DXL1_ID))
        quit()

    # Add Dynamixel#2 goal position value to the Syncwrite parameter storage
    dxl_addparam_result = ctypes.c_ubyte(
        dynamixel.groupSyncWriteAddParam(group_num, DXL2_ID,
                                         dxl_goal_position[index],
                                         LEN_MX_GOAL_POSITION)).value
    if dxl_addparam_result != 1:
        print("[ID:%03d] groupSyncWrite addparam failed" % (DXL2_ID))
# Add parameter storage for Dynamixel#7 present position value
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncReadAddParam(groupread_num, DXL7_ID)).value
if dxl_addparam_result != 1:
    print("[ID:%03d] groupSyncRead addparam failed" % (DXL7_ID))
    quit()


while 1:
    print("Press any key to continue! (or press ESC to quit!)")
    if getch() == chr(ESC_ASCII_VALUE):
        break

    for pose in range(0, 10):
        # Add Dynamixel#1 goal position value to the Syncwrite storage
        dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL1_ID, dxl_goal_position[pose][DXL1_ID - 1], LEN_XM430_GOAL_POSITION)).value
        if dxl_addparam_result != 1:
            print("[ID:%03d] groupSyncWrite addparam failed" % (DXL1_ID))
            quit()

        # Add Dynamixel#2 goal position value to the Syncwrite parameter storage
        dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL2_ID, dxl_goal_position[pose][DXL2_ID - 1], LEN_XM430_GOAL_POSITION)).value
        if dxl_addparam_result != 1:
            print("[ID:%03d] groupSyncWrite addparam failed" % (DXL2_ID))
            quit()

        # Add Dynamixel#3 goal position value to the Syncwrite parameter storage
        dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL3_ID, dxl_goal_position[pose][DXL3_ID - 1], LEN_XM430_GOAL_POSITION)).value
        if dxl_addparam_result != 1:
            print("[ID:%03d] groupSyncWrite addparam failed" % (DXL3_ID))
            quit()
Ejemplo n.º 13
0
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:
    print("Press any key to continue! (or press ESC to quit!)")
    if getch() == chr(ESC_ASCII_VALUE):
        break

    # Add Dynamixel#1 goal position value to the Syncwrite storage
    dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(group_num, DXL1_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION)).value
    print(dxl_addparam_result)
    if dxl_addparam_result != 1:
        print(dxl_addparam_result)
        print("[ID:%03d] groupSyncWrite addparam failed" % (DXL1_ID))
        quit()

    # Add Dynamixel#2 goal position value to the Syncwrite parameter storage
    dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(group_num, DXL2_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION)).value
    if dxl_addparam_result != 1:
        print("[ID:%03d] groupSyncWrite addparam failed" % (DXL2_ID))

    # Syncwrite goal position
    dynamixel.groupSyncWriteTxPacket(group_num)
    if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
        dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
Ejemplo n.º 14
0
    print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))

# Add parameter storage for Dynamixel present position value
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncReadAddParam(groupread_num, DXL_ID)).value
if dxl_addparam_result != 1:
    print("[ID:%03d] groupSyncRead addparam failed" % (DXL_ID))
    quit()


while 1:
    print("Press any key to continue! (or press ESC to quit!)")
    if getch() == chr(ESC_ASCII_VALUE):
        break

    # Add Dynamixel#1 goal position value to the Syncwrite storage
    dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID, dxl_goal_position[index], LEN_PRO_GOAL_POSITION)).value
    print(dxl_addparam_result)
    if dxl_addparam_result != 1:
        print("[ID:%03d] groupSyncWrite addparam failed" % (DXL_ID))
        quit()

    # Add Dynamixel#2 goal position value to the Syncwrite parameter storage
    dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID, dxl_led_value[index], LEN_PRO_LED_RED)).value
    if dxl_addparam_result != 1:
        print("[ID:%03d] groupSyncWrite addparam failed" % (DXL_ID))
        quit()

    # Syncwrite goal position
    dynamixel.groupSyncWriteTxPacket(groupwrite_num)
    dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
    if dxl_comm_result != COMM_SUCCESS:
# 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()


# Initialize Groupsyncwrite instance
groupwrite_acceleration_limit = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_XM430_ACCELERATION_LIMIT, LEN_XM430_ACCELERATION_LIMIT)

# Add Dynamixel #1~#5 acceleration limit value to the Syncwrite storage
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_acceleration_limit, DXL1_ID, dxl_acc_limit, LEN_XM430_ACCELERATION_LIMIT)).value
if dxl_addparam_result != 1:
    print("[ID:%03d] groupSyncWrite addparam failed" % (DXL1_ID))
    quit()

dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_acceleration_limit, DXL2_ID, dxl_acc_limit, LEN_XM430_ACCELERATION_LIMIT)).value
if dxl_addparam_result != 1:
    print("[ID:%03d] groupSyncWrite addparam failed" % (DXL2_ID))
    quit()

dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_acceleration_limit, DXL3_ID, dxl_acc_limit, LEN_XM430_ACCELERATION_LIMIT)).value
if dxl_addparam_result != 1:
    print("[ID:%03d] groupSyncWrite addparam failed" % (DXL3_ID))
    quit()

dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_acceleration_limit, DXL4_ID, dxl_acc_limit, LEN_XM430_ACCELERATION_LIMIT)).value
Ejemplo n.º 16
0
# Add parameter storage for Dynamixel present position value
dxl_addparam_result = ctypes.c_ubyte(
    dynamixel.groupSyncReadAddParam(groupread_num, DXL_ID)).value
if dxl_addparam_result != 1:
    print("[ID:%03d] groupSyncRead addparam failed" % (DXL_ID))
    quit()

while 1:
    print("Press any key to continue! (or press ESC to quit!)")
    if getch() == chr(ESC_ASCII_VALUE):
        break

    # Add Dynamixel#1 goal position value to the Syncwrite storage
    dxl_addparam_result = ctypes.c_ubyte(
        dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID,
                                         dxl_goal_position[index],
                                         LEN_PRO_GOAL_POSITION)).value
    print(dxl_addparam_result)
    if dxl_addparam_result != 1:
        print("[ID:%03d] groupSyncWrite addparam failed" % (DXL_ID))
        quit()

    # Add Dynamixel#2 goal position value to the Syncwrite parameter storage
    dxl_addparam_result = ctypes.c_ubyte(
        dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID,
                                         dxl_led_value[index],
                                         LEN_PRO_LED_RED)).value
    if dxl_addparam_result != 1:
        print("[ID:%03d] groupSyncWrite addparam failed" % (DXL_ID))
        quit()