Fawkes における Dynamixel のStatus Return Level の扱い

Fawkes の Dynamixel は,応答速度を高めるため,servo からの返事を「READ_DATAだけに応答する」に変更しています.
というか,強制上書きしています….

「すべてのインストラクションに応答する」で運用したい時もありますので,
cfg/conf.d/dynamixel.yaml で設定できるようにしました.

まず,yaml ファイルは,以下の通りです.

Index: btr2017/rcll2017/fawkes-robotino/cfg/conf.d/dynamixel.yaml
===================================================================
--- btr2017/rcll2017/fawkes-robotino/cfg/conf.d/dynamixel.yaml (revision 1760)
+++ btr2017/rcll2017/fawkes-robotino/cfg/conf.d/dynamixel.yaml (revision 1855)
@@ -68,2 +68,8 @@
     autorecover_enabled: true
     autorecover_flags: 127
+
+    # set return_level
+    # 0: SRL_RESPONDE_NONE
+    # 1: SRL_RESPOND_READ
+    # 2: SRL_RESPOND_ALL
+    return_level: 2

上記のreturn_level を扱う変数として,cfg_return_level_ を用意

Index: btr2017/rcll2017/fawkes-robotino/fawkes/src/plugins/dynamixel/driver_thread.h
===================================================================
--- btr2017/rcll2017/fawkes-robotino/fawkes/src/plugins/dynamixel/driver_thread.h (revision 1699)
+++ btr2017/rcll2017/fawkes-robotino/fawkes/src/plugins/dynamixel/driver_thread.h (revision 1855)
@@ -130,4 +130,5 @@
   float         cfg_max_voltage_;
   std::vector<unsigned int> cfg_servos_to_discover_;
+  unsigned char cfg_return_level_;
   
   void  goto_angle(unsigned int servo_id, float angle);

yaml から読み込んで変数に代入する部分と,初期化時にそれを利用する部分.

Index: btr2017/rcll2017/fawkes-robotino/fawkes/src/plugins/dynamixel/driver_thread.cpp
===================================================================
--- btr2017/rcll2017/fawkes-robotino/fawkes/src/plugins/dynamixel/driver_thread.cpp (revision 1800)
+++ btr2017/rcll2017/fawkes-robotino/fawkes/src/plugins/dynamixel/driver_thread.cpp (revision 1855)
@@ -86,4 +86,5 @@
   cfg_max_voltage_                      = config->get_float((cfg_prefix_ + "max_voltage").c_str());
   cfg_servos_to_discover_               = config->get_uints((cfg_prefix_ + "servos").c_str());
+  cfg_return_level_			= config->get_uint((cfg_prefix_ + "return_level").c_str());
 
   chain_ = new DynamixelChain(cfg_device_.c_str(), cfg_read_timeout_ms_, cfg_enable_echo_fix_, cfg_enable_connection_stability_, cfg_min_voltage_, cfg_max_voltage_);
@@ -145,6 +146,14 @@
   }
 
+  unsigned char cfg_return_levels_[] = {DynamixelChain::SRL_RESPOND_NONE, 
+	  				DynamixelChain::SRL_RESPOND_READ,
+					DynamixelChain::SRL_RESPOND_ALL};
+
+  /* 
   // We only want responses to be sent on explicit READ to speed up communication
   chain_->set_status_return_level(DynamixelChain::BROADCAST_ID, DynamixelChain::SRL_RESPOND_READ);
+  */
+  chain_->set_status_return_level(DynamixelChain::BROADCAST_ID, cfg_return_levels_[cfg_return_level_]);
+
   // set compliance values
   chain_->set_compliance_values(DynamixelChain::BROADCAST_ID,

これで,簡単に切り替えられます.

決して,0 ( = SRL_RESPOND_NONE)を指定しないこと!
指定すると,応答しなくなり,fawkes からも手出しできなくなります.
その時は,純正の設定ツールを使ってください(ただし,Windows のみで動作).

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です

*