...
POST No. 2406925
Setting DeviceID using DynamixelSDK on macOS?
2018-08-08 01:00:36 brandon

Hi,


I'm able to control a single XL430-W250 using this setup:


my_python <--> DynamixelSDK/python <--> macOS <--> usb <--> U2D2 <--> XL430 <-- SMPS <-- 12VDC supply


(test scripts work: ping.py, read_write.py etc)


I want to daisychain a second XL430, but I need to change it's deviceID to <not 1>.


All the instructions I can find involve either using a controller board or a WIndows PC.


Is there a script for setting the deviceID of a connected XL430?


Thanks,


Brandon

2018-08-08 01:00:36
brandon
2018-08-08 03:05:11 Kurt

Hopefully @Will Son will give a better answer.  As I don't use Python, don't own a U2D2 ...


But if it were me, and I had simple python stuff working. 


It would not be hard to then write a quick and dirty Python script to maybe read that servo #1 exists, and then update it's servo id:


use the function: write1ByteTxRx(port, dxl_id, address, data)


Using the protocol 2 handler, and your port object, where, you would for example pass in

dxl_id = 1

addres = 7  // register for ID

data = 2  (or whatever you want the new servos ID to be)


Note: I believe to write to this register which is in the EEPROM area, you need to make sure that torque is not enabled (address 64)

2018-08-08 03:05:11
Kurt
2018-08-08 09:48:45 Will Son

Hello Brandon,


Unfortunately, current Roboplus Manager 2.0 only supports Windows PC.

If you have OpenCM9.04 or OpenCR, you could use Dynamixel Monitor sketch for managing Dynamixel.

But if you only have U2D2, DynamixelSDK will be the best solution for now.


As Kurt recommended in the above reply, you can change ID with write1ByteTxOnly or write1ByteTxRx function defined in the protocol2_packet_handler.py file.

The address and size for ID(7 and 1byte respectively) can be found in the XL430-W250 Control Table.

Also please note that Data in the EEPROM Area can only be written when the value of Torque Enable(64) is cleared to ‘0’. 


What I recommend to do is:


1. Connect only one Dynamixel to change ID.

2. Check if torque for Dynamixel is turned off(simply supplying power to Dynamixel won't turn on the torque)

3. Transmit change ID instruction with either write1ByteTxOnly or write1ByteTxRx sudo ex) write1ByteTxOnly(your_U2D2_port, your_Dynamixel_ID, Address_for_ID, Desired_ID) #this will not return status packet.


Though I haven't used python, I think above steps will help you to change the address.

I'll request more examples that can modify ID and other properties of Dynamixel with python.

Thank you.

2018-08-08 09:48:45
Will Son
2018-08-14 20:41:58 Brandon

So - that was simple.  I had skimmed the docs and thought this was the method but it seems too easy.  Thanks both Kurt and Will Son for clarifying.


Here were my steps:

1. I first hooked up my U2D2 and SMPS to a single one of my Dynamixel XL430-W250 's and re-tested that   .../tests/read_write.py works (alway start with a working system.)

2. I made a copy of read_write.py as adjust_deviceID.py.

3. I edited adjust_deviceID.py, moving the Disable Dynamixel Torque block to the top (just below all the portHandler setup and tests under the setBaudRate stuff).  The new code in it's entirety is below*

4. Save, ran $ python adjust_deviceID.py in a terminal.  Nothing seemed to happen.

5. Edited read_write.py making DXL_ID = 2

6. $ python read_write.py.  --- worked like a champ.


*Code adjust_deviceID.py follows, but I had to comment out the #Open port block to get it to work.  I think it may be a macOS - serial port thing.  I will open another thread for this question.



#!/usr/bin/env python

# -*- coding: utf-8 -*-


################################################################################

# Copyright 2017 ROBOTIS CO., LTD.

#

# Licensed under the Apache License, Version 2.0 (the "License");

# you may not use this file except in compliance with the License.

# You may obtain a copy of the License at

#

#     http://www.apache.org/licenses/LICENSE-2.0

#

# Unless required by applicable law or agreed to in writing, software

# distributed under the License is distributed on an "AS IS" BASIS,

# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

# See the License for the specific language governing permissions and

# limitations under the License.

################################################################################


# Author: Ryu Woon Jung (Leon)


#

# *********     Read and Write Example      *********

#

#

# Available Dynamixel model on this example : All models using Protocol 2.0

# This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL.

# To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself.

# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600)

#

import ipdb

import os


if os.name == 'nt':

    import msvcrt

    def getch():

        return msvcrt.getch().decode()

else:

    import sys, tty, termios

    fd = sys.stdin.fileno()

    old_settings = termios.tcgetattr(fd)

    def getch():

        try:

            tty.setraw(sys.stdin.fileno())

            ch = sys.stdin.read(1)

        finally:

            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)

        return ch


from dynamixel_sdk import *                    # Uses Dynamixel SDK library


# Control table address

ADDR_PRO_TORQUE_ENABLE      = 64               # Control table address is different in Dynamixel model

ADDR_DEVICE_ID              = 7

ADDR_PRO_GOAL_POSITION      = 116

ADDR_PRO_PRESENT_POSITION   = 132


# Protocol version

PROTOCOL_VERSION            = 2.0               # See which protocol version is used in the Dynamixel


# Default setting

DXL_ID                      = 1                 # Dynamixel ID  - current device ID

DXL_ID_NEW                  = 2                 # Dynamixel ID  - change it to this one


BAUDRATE                    = 57600             # Dynamixel default baudrate : 57600

#DEVICENAME                  = '/dev/ttyUSB0'    # Check which port is being used on your controller

                                                # ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"

DEVICENAME                  = '/dev/tty.usbserial-FT2GXCAG'


TORQUE_ENABLE               = 1                 # Value for enabling the torque

TORQUE_DISABLE              = 0                 # Value for disabling the torque

DXL_MINIMUM_POSITION_VALUE  = 10           # Dynamixel will rotate between this value

DXL_MAXIMUM_POSITION_VALUE  = 4000            # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)

DXL_MOVING_STATUS_THRESHOLD = 20                # Dynamixel moving status threshold


index = 0

dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE]         # Goal position



# Initialize PortHandler instance

# Set the port path

# Get methods and members of PortHandlerLinux or PortHandlerWindows

portHandler = PortHandler(DEVICENAME)


# Initialize PacketHandler instance

# Set the protocol version

# Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler

packetHandler = PacketHandler(PROTOCOL_VERSION)


# Open port

# if portHandler.openPort():

#     print("Succeeded to open the port")

# else:

#     print("Failed to open the port")

#     print("Press any key to terminate...")

#     getch()

#     quit()



# Set port baudrate

if portHandler.setBaudRate(BAUDRATE):

    print("Succeeded to change the baudrate")

else:

    print("Failed to change the baudrate")

    print("Press any key to terminate...")

    getch()

    quit()



# Disable Dynamixel Torque

dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)

if dxl_comm_result != COMM_SUCCESS:

    print("%s" % packetHandler.getTxRxResult(dxl_comm_result))

elif dxl_error != 0:

    print("%s" % packetHandler.getRxPacketError(dxl_error))



# set device ID to N

dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_DEVICE_ID, DXL_ID_NEW)

if dxl_comm_result != COMM_SUCCESS:

    print("%s" % packetHandler.getTxRxResult(dxl_comm_result))

elif dxl_error != 0:

    print("%s" % packetHandler.getRxPacketError(dxl_error))



# Close port

portHandler.closePort()


2018-08-14 20:41:58
Brandon
Reply
웹에디터 시작 웹 에디터 끝