DynamixelをOpenCRとRasberryPiでキーボード操作するまでのメモ

# はじめに

DynamixelとOpenCRを使ったロボット制御のメモです。Turtlebot3と類似の構成ですが、モーターや電源が異なります。

LinuxPCとRasberryPiを使っていますが、LinuxPCだけでもOKです。RasberryPiでAruduinoIDEを使えなかったため、LinuxPCでArduinoIDEを使ってOpenCRのファームを書き換えました。逆に言えば、ArduinoIDEに関係ないものは、全てRasberryPiでもできます。

# 環境

| 項目 | バージョン |
|-- | -- | 
| Ubuntu|20.04|
|ROS|Noetic|

- LinuxPC (Ubuntu 20.04.6 LTS)：主にArduinoIDEの為に使用
- RasberryPi (Ubuntu 20.04.4 LTS) 
- [OpenCR](https://emanual.robotis.com/docs/en/parts/controller/opencr10_jp/#dynamixel-workbench)
- [Dynamixel PM42-010-S260-R](https://emanual.robotis.com/docs/en/dxl/p/pm42-010-s260-r/)
- 24V電源 (GSユアサ12VDCバッテリー*2)

# 参考
- [DYNAMIXEL Workbench (robotis.com)](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench_jp/)
- [DynamixelをROSで動かせるようにした話(1) 環境構築編 #ROS1 - Qiita](https://qiita.com/FAL19/items/e42b4a86ae026bdb46e7)

# 作業手順
## 接続

OpenCRのRS485のコネクタの左右がPM42-010-S260-Rと接続されています。電源ジャックには24VDCが接続されます。USBはLinuxPC、もしくはRasberryPiに接続されます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/1ece237f-94aa-df4b-a6ed-10b9ad9c9251.png)
※ [OpenCR](https://emanual.robotis.com/docs/en/parts/controller/opencr10_jp/#dynamixel-workbench)

## ROSパッケージをダウンロードする
ROS noeticがすでにインストールされており、catkin_ws 以下で作業する前提です。

```bash
$ mkdir ~/catkin_ws/src/dynamixel
$ cd ~/catkin_ws/src/dynamixel

# 主なパッケージ
$ git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench.git
$ git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench-msgs.git

# 依存パッケージ
$ git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git
```

## Arduino IDE 
※ LinuxPC上で作業しました。

### Arduino IDE インストール
OpenCRにファームウェアをアップロードするために、ArduinoIDEをダウンロードします。

### USBポート設定
```bash
$ wget https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/99-opencr-cdc.rules
$ sudo cp ./99-opencr-cdc.rules /etc/udev/rules.d/
$ sudo udevadm control --reload-rules
$ sudo udevadm trigger
```

### コンパイラ設定
```bash
$ sudo apt-get install libncurses5-dev:i386
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/f51351ff-d493-e12e-056c-964af64f3f2a.png)
このような画面になるので、OKして次に進みます。

### ArduinoIDE実行
```bash
$ arduino
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/7ba2c386-59de-9651-14f0-6078a545f95c.png)
直近でusb_to_dxlを開いていたため、この画面が出ています。

### Arduino IDEへの環境移植
#### 環境設定
上部メニューで [ファイル] → [環境設定] をクリックします。「環境設定」ウィンドウが表示されたら、次のリンクをコピーして「追加のボードマネージャのURL」テキストボックスに貼り付けます。
```
https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/arduino/opencr_release/package_opencr_index.json
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/f9241e71-19c5-2dfd-2072-42d157e88e66.png)

#### ボードマネージャー経由でボードパッケージをインストール
「ツール」→「ボード」→「ボードマネージャー」をクリックします。OpenCRをインストールします。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/c199c3fe-2c34-8bd1-6ac9-145f9f8dd619.png)

OpenCR Board が [ツール] → [Board] のリストにあるかどうかを確認します。これをクリックして OpenCR Board ソースをインポートします。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/ad3674f4-ac0d-304f-dcaf-0caf4f1a7cfb.png)

#### ポート設定
OpenCRとLinuxPCがシリアル接続されている前提です。下記コマンドでttyACM0 として認識していることを確認します。
```bash
$ ls /dev/ttyACM0*
/dev/ttyACM0
```

Arduinoの「ツール」→「ポート」→「/dev/ttyACM0」を選択します。

#### モードマネージャを削除します。
```bash
$ sudo apt-get purge modemmanager
```

### Arduino IDEでOpenCMにusb_to_dxlを書き込む

[ファイル] → [スケッチ例] → [OpenCR] → [10.Etc] → [usb_to_dxl]を開きます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/93234083-6f95-c292-f099-d6ad5b566888.png)

マイコンボードに書き込みます。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/895ee0a3-7b45-8703-cef0-1b59bf387cd6.png)

## 接続の確認 (find_dynamixel)

このノードは全てのIDをそれぞれのBaudrate(9600, 57600, 115200, 1000000, 2000000, 3000000, 4000000)ごとにスキャンし、接続しているDynamixelの数を表示します。
以下では、異なるBaudrateで2つのDynamixelが検出されています。

```bash
$ rosrun dynamixel_workbench_controllers find_dynamixel /dev/ttyACM0
[ INFO] [1702276278.330805730]: Succeed to init(9600)
[ INFO] [1702276278.330854910]: Wait for scanning...
[ INFO] [1702276300.113791468]: Find 0 Dynamixels
[ INFO] [1702276300.155892835]: Succeed to init(57600)
[ INFO] [1702276300.155936602]: Wait for scanning...
[ INFO] [1702276318.156320385]: Find 1 Dynamixels
[ INFO] [1702276318.156378417]: id : 2, model name : PRO-PLUS-M42P-010-S260-R
[ INFO] [1702276318.213621680]: Succeed to init(115200)
[ INFO] [1702276318.213664241]: Wait for scanning...
[ INFO] [1702276335.871239691]: Find 0 Dynamixels
[ INFO] [1702276335.917334827]: Succeed to init(1000000)
[ INFO] [1702276335.917379935]: Wait for scanning...
[ INFO] [1702276353.212068238]: Find 1 Dynamixels
[ INFO] [1702276353.212111676]: id : 1, model name : PRO-PLUS-M42P-010-S260-R
[ INFO] [1702276353.268963791]: Succeed to init(2000000)
[ INFO] [1702276353.268978956]: Wait for scanning...
[ INFO] [1702276370.574371296]: Find 0 Dynamixels
[ INFO] [1702276370.619668935]: Succeed to init(3000000)
[ INFO] [1702276370.619712521]: Wait for scanning...
[ INFO] [1702276387.919237066]: Find 0 Dynamixels
[ INFO] [1702276387.964325078]: Succeed to init(4000000)
[ INFO] [1702276387.964366980]: Wait for scanning...
[ INFO] [1702276405.260781136]: Find 0 Dynamixels
```

Baudrateを変更したい場合は「ファイル」→「スケッチ例」→「OpenCR」→「08.DynamixelWorkbench」→「d_BPS_Change」
冒頭を任意のIDやBaudrateに変更します。
```
	#define BAUDRATE  57600
	#define DXL_ID 2
	#define NEW_BAUDRATE 115200
```

その後、電源リセット、usb_to_dxlを書き直して、同じBaudrateで認識するようになった。
```bash
$ rosrun dynamixel_workbench_controllers find_dynamixel /dev/ttyACM0
[ INFO] [1702353668.641122756]: Succeed to init(9600)
[ INFO] [1702353668.641159667]: Wait for scanning...
[ INFO] [1702353690.424888613]: Find 0 Dynamixels
[ INFO] [1702353690.466194675]: Succeed to init(57600)
[ INFO] [1702353690.466233049]: Wait for scanning...
[ INFO] [1702353708.499108720]: Find 0 Dynamixels
[ INFO] [1702353708.540837247]: Succeed to init(115200)
[ INFO] [1702353708.540853263]: Wait for scanning...
[ INFO] [1702353726.135644295]: Find 2 Dynamixels
[ INFO] [1702353726.135703111]: id : 1, model name : PRO-PLUS-M42P-010-S260-R
[ INFO] [1702353726.135730202]: id : 2, model name : PRO-PLUS-M42P-010-S260-R
[ INFO] [1702353726.207482482]: Succeed to init(1000000)
[ INFO] [1702353726.207547353]: Wait for scanning..
```

## キーボードでDymamixelを動かす

### 設定ファイルの準備
適当にyamlファイルを作成します。
`~\catkin_ws\src\dynamixel\dynamixel-workbench\dynamixel_workbevf nch_controllers\config\my_config.yaml`を作成します。
Operating Modeには1 # VELOCITY_CONTROL_MODEを設定します。

```bash
Motor1:
  ID: 1
  Return_Delay_Time: 0
  Operating_Mode: 1 #  VELOCITY_CONTROL_MODE
  Profile_Acceleration: 0
  Profile_Velocity: 0
Motor2:
  ID: 2
  Return_Delay_Time: 0
  Operating_Mode: 1 #  VELOCITY_CONTROL_MODE
  Profile_Acceleration: 0
  Profile_Velocity: 0
# static const uint8_t CURRENT_CONTROL_MODE                  = 0;
# static const uint8_t VELOCITY_CONTROL_MODE                 = 1;
# static const uint8_t POSITION_CONTROL_MODE                 = 3;
# static const uint8_t EXTENDED_POSITION_CONTROL_MODE        = 4;
# static const uint8_t CURRENT_BASED_POSITION_CONTROL_MODE   = 5;
# static const uint8_t PWM_CONTROL_MODE                      = 16;
# static const uint8_t TORQUE_CONTROL_MODE                   = 100;
# static const uint8_t MULTI_TURN_MODE                       = 101;
```

launchファイル`~/catkin_ws\src\dynamixel\dynamixel-workbench\dynamixel_workbench_controllers\launch\dynamixel_controllers.launch`を編集します。

```
	<launch>
	  <arg name="usb_port"                default="/dev/ttyACM0"/>
	  <arg name="dxl_baud_rate"           default="115200"/>
	  <arg name="namespace"               default="dynamixel_workbench"/>
	  <arg name="use_moveit"              default="false"/>
	  <arg name="use_joint_state"         default="true"/>
	  <arg name="use_cmd_vel"             default="true"/>
	  <param name="dynamixel_info"          value="$(find dynamixel_workbench_controllers)/config/my_config.yaml"/>
	  <node name="$(arg namespace)" pkg="dynamixel_workbench_controllers" type="dynamixel_workbench_controllers"
	        required="true" output="screen" args="$(arg usb_port) $(arg dxl_baud_rate)">
	    <param name="use_moveit"              value="$(arg use_moveit)"/>
	    <param name="use_joint_states_topic"  value="$(arg use_joint_state)"/>
	    <param name="use_cmd_vel_topic"       value="$(arg use_cmd_vel)"/>
	    <rosparam>
	      publish_period: 0.010
	      dxl_read_period: 0.010
	      dxl_write_period: 0.010
	      mobile_robot_config:                <!--this values will be set when 'use_cmd_vel' is true-->
	        seperation_between_wheels: 0.300  
	        radius_of_wheel: 0.0785           
	    </rosparam>
	  </node>
	</launch>
```
### キーボード操作のPackageのインストールします。

```bash
$ sudo apt-get install ros-noetic-teleop-twist-keyboard
```
参考：[キーボードでロボット操作 - teleop_twist_keyboard - クワマイでもできる (hatenablog.com)](https://kuwamai.hatenablog.com/entry/2019/05/09/200000)

### キーボード操作

ターミナルでコントローラを実行します。
```bash
$ roslaunch dynamixel_workbench_controllers dynamixel_controllers.launch
```

別のターミナルでトピックリストを確認して、cmd_velが存在することを確認します。
```bash
$ rostopic list
/dynamixel_workbench/cmd_vel
/dynamixel_workbench/dynamixel_state
/dynamixel_workbench/joint_states
/dynamixel_workbench/joint_trajectory
/rosout
/rosout_agg
```bash

teleop_twist_keyboard 実行時の引数のcmd_velを上記に設定して、teleop_twist_keyboardを実行します。

```bash
$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/dynamixel_workbench/cmd_vel
```

キーボード操作しながら、別のターミナルでトピックを監視すると、値が変わっていることがわかります。これはgeometry_msgs::Twist型のメッセージです。

```bash
$ rostopic echo /dynamixel_workbench/cmd_vel
linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0
```

以下は2つのターミナルを並べて動作させている様子です。キーボードを操作すると、モータ動作に加えて、Twist型のメッセージの内容が変化していることがわかります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/599506/6e681a5e-8bce-96bb-4479-19c6449cd88c.png)

# さいごに

本記事では差動二輪のロボットをROSで動かすために、まずはキーボードで操作できる環境構築のメモを記載しました。