Go to Qiita Advent Calendar 2024 Top
LoginSignup
6
5

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@FAL19

DynamixelをOpenCRとArduinoIDEで動かしてみる

Last updated at Posted at 2019-09-02

#概要
DynamixelモータをUbuntu上のArduinoIDEとOpenCR1.0，DynamixelSDKを用いて動かしてみた.
インストール方法はROBOTISのe-manual等を参照．

#実行環境

OS モータ
Ubuntu16.04 LTS DYNAMIXEL XM540-W270-R

#目次

  1. Arduino IDEのサンプルプログラムを見てみる
  2. XM540-W270-RのControl Tableを詳しく見てみる
  3. 実際に動かしてみる
  4. まとめ
  5. 参考リンク

#1. Arduino IDEのサンプルプログラムを見てみる
###1-1. サンプルプログラムの読み込み
Arduino IDEを起動し, 「File(ファイル)」⇒「Examples(スケッチ例)」⇒「OpenCR」⇒「07. DynamixelSDK」⇒「protocol1.0」⇒「read_write」を開く.

read_write
#include <DynamixelSDK.h>


// Control table address
#define ADDR_MX_TORQUE_ENABLE           24                  // Control table address is different in Dynamixel model
#define ADDR_MX_GOAL_POSITION           30
#define ADDR_MX_PRESENT_POSITION        36

// Protocol version
#define PROTOCOL_VERSION                1.0                 // See which protocol version is used in the Dynamixel

// Default setting
#define DXL_ID                          1                   // Dynamixel ID: 1
#define BAUDRATE                        57600
#define DEVICENAME                      "OpenCR_DXL_Port"   // This definition only has a symbolic meaning and does not affect to any functionality


#define TORQUE_ENABLE                   1                   // Value for enabling the torque
#define TORQUE_DISABLE                  0                   // Value for disabling the torque
#define DXL_MINIMUM_POSITION_VALUE      100                 // Dynamixel will rotate between this value
#define 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.)
#define DXL_MOVING_STATUS_THRESHOLD     10                  // Dynamixel moving status threshold

#define CMD_SERIAL                      Serial

int getch(){
  while(1){
    if( CMD_SERIAL.available() > 0 ){
      break;
    }
  }

  return CMD_SERIAL.read();
}

int kbhit(void){
  return CMD_SERIAL.available();
}

void setup(){
  Serial.begin(115200);
  while(!Serial);

  Serial.println("Start..");

  // Initialize PortHandler instance
  // Set the port path
  // Get methods and members of PortHandlerLinux or PortHandlerWindows
  dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);

  // Initialize PacketHandler instance
  // Set the protocol version
  // Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
  dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);

  int index = 0;
  int dxl_comm_result = COMM_TX_FAIL;             // Communication result
  int dxl_goal_position[2] = {DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE};         // Goal position

  uint8_t dxl_error = 0;                          // Dynamixel error
  uint16_t dxl_present_position = 0;              // Present position

  // Open port
  if (portHandler->openPort()){
    Serial.print("Succeeded to open the port!\n");
  }
  else{
    Serial.print("Failed to open the port!\n");
    return;
  }

  // Set port baudrate
  if (portHandler->setBaudRate(BAUDRATE)){
    Serial.print("Succeeded to change the baudrate!\n");
  }
  else{
    Serial.print("Failed to change the baudrate!\n");
    return;
  }

  // Enable Dynamixel Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS){
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }
  else{
    Serial.print("Dynamixel has been successfully connected \n");
  }

  while(1){
    Serial.print("Press any key to continue! (or press q to quit!)\n");
    if (getch() == 'q')
      break;

    // Write goal position
    dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index], &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS){
      Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
    }
    else if (dxl_error != 0){
      Serial.print(packetHandler->getRxPacketError(dxl_error));
    }

    do{
      // Read present position
      dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, DXL_ID, ADDR_MX_PRESENT_POSITION, &dxl_present_position, &dxl_error);
      if (dxl_comm_result != COMM_SUCCESS){
        Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
      }
      else if (dxl_error != 0){
        Serial.print(packetHandler->getRxPacketError(dxl_error));
      }

      Serial.print("[ID:"); Serial.print(DXL_ID);
      Serial.print("] GoalPos:"); Serial.print(dxl_goal_position[index]);
      Serial.print("  PresPos:"); Serial.print(dxl_present_position);
      Serial.println(" ");

    }while((abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD));

    // Change goal position
    if (index == 0){
      index = 1;
    }
    else{
      index = 0;
    }
  }

  // Disable Dynamixel Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS){
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  // Close port
  portHandler->closePort();
}

void loop(){
}

このプログラムは, モータの現在位置をシリアルモニタに出力するというプログラムである.

ちなみに, コメント文にあるようにこれはDynamixel MX-28用のサンプルプログラムであり, XM540-W270-Rを動かす際には少し手直しを行わなければいけないので注意が必要.

重要そうな部分を見ていく.

###1-2. 値やアドレスの定義

#include <DynamixelSDK.h>


// Control table address
#define ADDR_MX_TORQUE_ENABLE           24                  // Control table address is different in Dynamixel model
#define ADDR_MX_GOAL_POSITION           30
#define ADDR_MX_PRESENT_POSITION        36

// Protocol version
#define PROTOCOL_VERSION                1.0                 // See which protocol version is used in the Dynamixel

// Default setting
#define DXL_ID                          1                   // Dynamixel ID: 1
#define BAUDRATE                        57600
#define DEVICENAME                      "OpenCR_DXL_Port"   // This definition only has a symbolic meaning and does not affect to any functionality


#define TORQUE_ENABLE                   1                   // Value for enabling the torque
#define TORQUE_DISABLE                  0                   // Value for disabling the torque
#define DXL_MINIMUM_POSITION_VALUE      100                 // Dynamixel will rotate between this value
#define 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.)
#define DXL_MOVING_STATUS_THRESHOLD     10                  // Dynamixel moving status threshold

#define ESC_ASCII_VALUE                 0x1b

#define CMD_SERIAL                      Serial

この部分ではデータを書き込むアドレスや書き込む値を定義(define)している.

Dymamixelモータでは基本的にモータと通信を行い, 指定されたアドレスに値を書き込むことによって動作をさせる.
また, 「どこに値を書き込めばいいのか」というような情報は, 商品ページのControl Tableというアドレス一覧表が通常は公開されている.

今回のサンプルではMX-28が使用されているので, MX-28のマニュアルページを見てみる.

MX-28 マニュアルページ

ページを開くと, そのページの真ん中より少し下に画像のような表がある. これがControl Tableである.

MX-28 ControlTable.PNG

この表を見れば, 「状態を変更するにはどのアドレスに書き込めばいいのか」「書き込む値の範囲」「何byteのデータを書き込めばいいのか」がわかる.

サンプルスケッチを見てみると,
「TORQUE_ENABLE」のアドレス = 24
「GOAL_POSITION」のアドレス = 30
「PRESENT_POSITION」のアドレス = 36
と定義されている.

ここで, 再びMX-28のControl Tableを見てみる.
MX-28 ControlTable1.PNG

画像を見てわかるように, Control Table通りにアドレスを定義していることがわかる.
つまり, XM540-W270-Rを動かす際は, XM540-W270-RのControl Tableで記載されているアドレスを定義する必要がある.

###1-3. PortとPacketのインスタンス生成

  // Initialize PortHandler instance
  // Set the port path
  // Get methods and members of PortHandlerLinux or PortHandlerWindows
  dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);

  // Initialize PacketHandler instance
  // Set the protocol version
  // Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
  dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);

この部分ではDynamixelSDKによって提供されるPortHandlerとPacketHandlerのインスタンスを生成している.
また, インスタンスの引数であるDEVICENAMEとPROTOCOL_VERSIONは1-2で挙げた部分で定義がされている.

基本的にはDEVICENAMEにはOpenCRとのシリアルポート名を, PROTOCOL_VERSIONにはモータとの通信プロトコルのバージョンを定義しておけばよい.
不明な場合は商品マニュアルを参照.

###1-4. シリアルポートの開放と通信速度設定

  // Open port
  if (portHandler->openPort()){
    Serial.print("Succeeded to open the port!\n");
  }
  else{
    Serial.print("Failed to open the port!\n");
    return;
  }

  // Set port baudrate
  if (portHandler->setBaudRate(BAUDRATE)){
    Serial.print("Succeeded to change the baudrate!\n");
  }
  else{
    Serial.print("Failed to change the baudrate!\n");
    return;
  }

この部分ではDynamixelとOpenCRとの通信を開始するために必要なことをしている.

###1-5. Dynamixelのトルクを有効化する

  // Enable Dynamixel Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS){
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }
  else{
    Serial.print("Dynamixel has been successfully connected \n");
  }

TORQUE ENABLEの値を書き込みことによって, モータがトルクをかけ始めるようになる, つまりモータを起動させることができる.
if文はErrorが出たかどうかの表示を行っている(あまり気にしなくてもいい？)

Enable Torqueの書き込みにはPacketHandlerのwrite1ByteTxRx()を使って書き込みを行う.

write1ByteTxRx(portHandler, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);

ここで, 第一引数にはPortHandlerのアドレス, 第二引数にはDynaixelのID, 第三引数には書き込みたい部分のアドレス, 第四引数には書き込みたい値, 第五引数にはerror変数のアドレスを入れる. 詳細はDynamixelSDK.hを参照.

なお, 今回のTorque EnableはControl Tabelによると1バイトのデータが記録されているため, これの値を書き換える際に1byteの書き込み(write)をする関数write1ByteTxRx()を使用したが, Goal PositionやPresent Positionなど, 2ビットの値を読み書きする場合は別の関数が用意されている(write2ByteTxRx()やread2ByteTxRx())ため, Control Tableから何バイトのデータを書き込めば良いのかよく確認する必要がある. これは4ビットの場合も同様.

###1-6. サンプルから分かること
以上から, 次のことが分かった.

・Control Tableに記載されたアドレスに値を書き込めば, モータの状態を変更することができる.
・書き込みにはPortHandlerのwrite*ByteTxRx関数を使用すればよい.
・読み込みにはRead*ByteTxRx関数を使用すればよい.

次の章では実際に動かしたいXM540-W270-RのControl Tableを見て具体的な動かし方を説明する.

###1-7. コンパイルがうまくいかない場合
・DynamixelSDK.hが見つからない場合
　⇒Arduinoの「ツール」⇒「ボード」から「OpenCR」を選択すればDynamixelSDK.hが読み込まれる.

#2. XM540-W270-RのControl Tableを詳しく見てみる
XM540-W270-RのControl Tableのうち重要な部分を説明する.
詳細はControl Tableの下にある説明を参照のこと.
XM540-W270-R マニュアルページ

###2-1. Operation Mode (Address:11)
このアドレスに適切な値を書き込むと, モータの動作モードを変更することができる.
基本的な動作モードは
・Current Control Mode (電流制御)
・Velocity Control Mode (速度制御)
・Position Control Mode (位置制御)
・PWM Control Mode (PWM制御)

位置制御と速度制御に関しては, PIDゲイン(Position Gain, Velocity Gain)と目標値(Goal Position, Goal Velocity)によってPID制御が行われ, PWM制御では目標duty比(Goal PWM)を書き込むことで制御が行われる.

###2-2. LED (Address:65)
XM540-W270-RにはLEDが搭載されており, 自由なタイミングでLEDを転倒させることが出来る.
LEDを光らせることでデバッグに役立つかもしれない.

###2-3. Torque Enable (Address:64)
モータにトルクをかけるかどうかを制御できる.
ただし, Control Table下の注意書きにも書いてあるが,

Control TableのAccessに(NVM)とついている物の値の書き換えはTorque Enableが0の場合のみ可能

とあるので, Operation Mode や値の制限関係(~ Limit)の書き換えを行う際はTorque EnableをONにする前に行ったほうが良い.

###2-4. 現在値の読み込み
モータの現在の状態を読み込みたい場合は, Control TableのPresent ~の部分を読みだせばよい.
基本的にControl Tableの黄色い部分が読み取り専用のアイテムである.

#3. 実際に動かしてみる
以上を踏まえてPWM制御と位置制御を行うプログラムを作った.

ここで, DEVICENAME にはシリアルポート名であるdev/ttyACM0, PROTOCOL_VERSIONにはモータの仕様により2.0と定義している.

###3-1. PWM制御

XM540-W270-R_PWM.ino
// DXL XM540-W270-R using Protocol 2.0
// This example is tested with a DXL XM540-W270-R, and an OpenCR

#include <DynamixelSDK.h>

//Control Table Address
#define ADDR_OPERATING_MODE           11
#define ADDR_MOVING_SPEED             32
#define ADDR_TORQUE_ENABLE            64
#define LED                           65
#define ADDR_GOAL_PWM                 100
#define ADDR_GOAL_POSITION            116
#define ADDR_PRESENT_POSITION         132

//Protocol Version
#define PROTOCOL_VERSION              2.0

//Default Setting
#define DXL_ID                        1
#define BAUDRATE                      57600
#define DEVICENAME                    "/dev/ttyACM0"


//Value Settings
#define TORQUE_ENABLE                 1
#define TORQUE_DISABLE                0
#define DXL_MINIMUM_POSITION_VALUE    400
#define DXL_MAXIMUM_POSITION_VALUE    4000
#define DXL_MOVING_STATUS_THRESHOLD   10
#define PWM_CONTROL_MODE              16
#define TARGET_DUTY_RATIO             500   //0~855

#define CMD_SERIAL                    Serial

int index = 0;
int dxl_comm_result = COMM_TX_FAIL;             // Communication result

uint8_t dxl_error = 0;                          // Dynamixel error

// Initialize PortHandler instance
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
dynamixel::PortHandler *portHandler

// Initialize PacketHandler instance
// Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
dynamixel::PacketHandler *packetHandler


int getch(){
  while(1){
    if(CMD_SERIAL.available()>0){
      break;
    }
  }
  return CMD_SERIAL.read();
}


int kbhit(void){
  return CMD_SERIAL.available();
}

void setup() {
  Serial.begin(115200);
  while(!Serial);

  Serial.println("Start..");

  // Set the port path
  // Get methods and members of PortHandlerLinux or PortHandlerWindows
  portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);

  // Set the protocol version
  // Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
  packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);

  // Open port
  if (portHandler->openPort()){
    Serial.print("Succeeded to open the port!\n");
  }
  else{
    Serial.print("Failed to open the port!\n");
    return;
  }

  // Set port baudrate
  if (portHandler->setBaudRate(BAUDRATE)){
    Serial.print("Succeeded to change the baudrate!\n");
  }
  else{
    Serial.print("Failed to change the baudrate!\n");
    return;
  }


  // Setting Operating Mode to Pwm-Mode
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_OPERATING_MODE, PWM_CONTROL_MODE, &dxl_error);
  if(dxl_comm_result != COMM_SUCCESS){
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }
  else{
    Serial.print("Operating Mode has been set to PWM Control Mode\n");
  }

  
  // Enable Dynamixel Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS){
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }
  else{
    Serial.print("Dynamixel has been successfully connected \n");
  }

  Serial.print("Press any key to continue! (or press q to quit!)\n");
  if (getch() == 'q'){
     break;
  }

void loop() {
  //Write Duty Ratio (PWM)
  dxl_comm_result = packetHandler -> write2ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_PWM, TARGET_DUTY_RATIO, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS){
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }
}

モータをDuty比=500/855[%]で回す

###3-2. 位置制御

XM540-W270-R_PositionControl.ino
#include <DynamixelSDK.h>
/*DEFINITIONS*/
//Control Table Address
#define ADDR_OPERATING_MODE           11
#define ADDR_MOVING_SPEED             32
#define ADDR_MAX_POSITION_LIMIT       48
#define ADDR_MIN_POSITION_LIMIT       52
#define ADDR_TORQUE_ENABLE            64
#define ADDR_LED                      65
#define ADDR_POSITION_D_GAIN          80    //0~16383
#define ADDR_POSITION_I_GAIN          82
#define ADDR_POSITION_P_GAIN          84
#define ADDR_GOAL_PWM                 100
#define ADDR_GOAL_POSITION            116
#define ADDR_PRESENT_POSITION         132

//Protocol Version
#define PROTOCOL_VERSION              2.0

//Default Setting
#define DXL_ID                        1
#define BAUDRATE                      57600
#define DEVICENAME                    "/dev/ttyACM0"

//Value Settings
#define TORQUE_ENABLE                 1
#define TORQUE_DISABLE                0
#define LED_ON                        1
#define LED_OFF                       0
#define MIN_POSITION_VALUE            0   //0~4095
#define MAX_POSITION_VALUE            4095
#define POSITION_THRESHOLD            5    

#define POSITION_P_GAIN               1000
#define POSITION_I_GAIN               0
#define POSITION_D_GAIN               500

//Operating Mode Setting
#define POSITION_CONTROL_MODE         3

#define TARGET_DUTY_RATIO             855   //Duty[%] = Value * 100 / 855

#define CMD_SERIAL                    Serial


/*PARAMETERS*/
int dxl_comm_result = COMM_TX_FAIL;             // Communication result
int target_position[] = {0, 2048};
uint32_t present_position = 0;
bool mode = false;

/*INSTANCES*/
//Initialize PortHandler instance
//Set the Port path
//Set methods and members of PortHandlerLinux or PortHandlerWindows
dynamixel::PortHandler *portHandler;
  
// Initialize PacketHandler instance
// Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
dynamixel::PacketHandler *packetHandler;

uint8_t dxl_error = 0;                          // Dynamixel error


int getch(){
  while(1){
    if(CMD_SERIAL.available()>0){
      break;
    }
  }
}


void setup() {
  Serial.begin(115200);
  while(!Serial);

  Serial.println("Start..");

  
/*START SETTINGS*/
  portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
  packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);

  
  // Open port
  if (portHandler->openPort()){
    Serial.print("Succeeded to open the port!\n");
  }
  else{
    Serial.print("Failed to open the port!\n");
    return;
  }

  // Set port baudrate
  if (portHandler->setBaudRate(BAUDRATE)){
    Serial.print("Succeeded to change the baudrate!\n");
  }
  else{
    Serial.print("Failed to change the baudrate!\n");
    return;
  }


  // Setting Operating Mode to Position-Control-Mode
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_OPERATING_MODE, POSITION_CONTROL_MODE, &dxl_error);
  if(dxl_comm_result != COMM_SUCCESS){
    Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.println(packetHandler->getRxPacketError(dxl_error));
  }
  else{
    Serial.println("Operating Mode has been set to Position Control Mode");
  }




/*PID GAIN SETTINGS*/
  // Setting Position P Gain
  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_POSITION_P_GAIN, POSITION_P_GAIN, &dxl_error);
  if(dxl_comm_result != COMM_SUCCESS){
    Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.println(packetHandler->getRxPacketError(dxl_error));
  }
  else{
    Serial.println("P Gain has been set");
  }

  // Setting Position I Gain
  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_POSITION_I_GAIN, POSITION_I_GAIN, &dxl_error);
  if(dxl_comm_result != COMM_SUCCESS){
    Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.println(packetHandler->getRxPacketError(dxl_error));
  }
  else{
    Serial.println("I Gain has been set");
  }

  // Setting Position D Gain
  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_POSITION_D_GAIN, POSITION_D_GAIN, &dxl_error);
  if(dxl_comm_result != COMM_SUCCESS){
    Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.println(packetHandler->getRxPacketError(dxl_error));
  }
  else{
    Serial.println("D Gain has been set");
  }


/*POSITION LIMIT SETTINGS*/
  //Max Position Limit
  dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, DXL_ID, ADDR_MAX_POSITION_LIMIT, MAX_POSITION_VALUE, &dxl_error);
  if(dxl_comm_result != COMM_SUCCESS){
    Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.println(packetHandler->getRxPacketError(dxl_error));
  }
  else{
    Serial.println("Maximun Position Limit has been set");
  }

  //Min Position Limit
  dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, DXL_ID, ADDR_MIN_POSITION_LIMIT, MIN_POSITION_VALUE, &dxl_error);
  if(dxl_comm_result != COMM_SUCCESS){
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }
  else{
    Serial.print("Minimum Position Limit has been set");
  }


  
  // Enable Dynamixel Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS){
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }
  else{
    Serial.print("Dynamixel has been successfully connected \n");
  }


  // LED
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_LED, LED_ON, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS){
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }
  else{
    Serial.print("LED has been turned on \n");
  }



  Serial.print("Press any key to continue!\n");
  getch();
}

void loop() {
  // Read Current Position
  dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, DXL_ID, ADDR_PRESENT_POSITION, &present_position, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS){
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  Serial.print("Present Position:");
  Serial.println(present_position);
  

  // Write Goal Position
  dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION, target_position[mode], &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS){
    Serial.print(packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0){
    Serial.print(packetHandler->getRxPacketError(dxl_error));
  }

  //Compare Goal and Present Position
  if(abs(present_position - target_position[mode]) < POSITION_THRESHOLD){
    mode = !mode;
  }
}

2つのmodeを切り替えて目標位置を行ったり来たりするプログラム.

#4. まとめ
・動作モード, 目標値などはControl Tableに記載されているアドレスに値を書き込む
・ポートの開放には「portHandler」クラス, データの送受信は「packetHandler」クラスを用いて行う.

今回はROSを使わずに動かしてみましたが，ROBOTISのgitに様々なROSのライブラリがあり，とても便利なのでROSで動かすことを強くお勧めします．
（この記事を書いた時にはROS使えなかった...(涙)）

#5. 参考リンク
[1] ROBOTIS e-Manual DYNAMIXEL_WORKBENCH
[2] ROBOTIS e-Manual DYNAMIXEL_Wizard2.0

6
5
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
6
5

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?