2. 最先端サーボモーター「Dynamixel」を自由自在に動かそう！

講座シリーズ｜
超小型PCラジコンカーで学ぶPythonプログラミング！
ミニロボットアームで学ぶPythonプログラミング！
（前回）< 1. 最速習得！Pythonでプログラミング自由自在！ - Qiita

（当記事は岐阜大学ロボコンサークルがプログラミング入門講座として公開している記事の一つです。一般の技術記事としても問題なくお読みいただけます。）

はじめに

岐阜大学ロボコンサークルです。
今回はスマートアクチュエータのDynamixelを実際にパソコンから操作することを目標にその使い方を勉強していきます！

使用機器

• Dynamixel Xシリーズ（XL320を除く）
• U2D2
• U2D2 PHB Set
• 12V ACアダプター

それぞれ組み立てて下写真のように接続しています。

動かす前の下準備

MasaYasuda/Dynamixel_tutorial
上のGitHubからZipファイルをダウンロードしましょう。
このファイルには製造側が提供しているDynamixel_sdkというものとそれを便利に扱うためのモジュール、及びサンプルコードが含まれています。

ダウンロード出来たらそれを解凍したフォルダをVSCodeで開き、DynamixelSDKフォルダの中にあるpythonフォルダを右クリック →「統合ターミナルで開く」を選択します。

ターミナルが開いたらそこでpython setup.py installと入力し セットアップをします。
（※ このとき、実行したpythonのversion（例：3.10.5 ('base') conda）は覚えておいてください。以後そのversionでしかdynamixel_sdkは機能しません。）
それっぽい表示が出たらOKです。

まずは動かそう！

では動かしていきましょう。
U2D2 Hub Boardに電源を差し電源をON、U2D2とPCをUSBでつないで、PC上でデバイスマネージャーを開きます。
（Windowsの検索バーから検索）

次に「ポート（COMとLPT）」をクリックして「COM？」と表示されている番号を確認します。

※ ここで「ポート（COMとLPT）」が表示されず、「その他」に「usb>> serial-converter」が表示されている場合
→　CDM212364_Setup.zip (2.2 MB)　←　ここをクリックして、Zipをダウンロードして解凍する（右クリック→「すべて展開」→OK→.zipファイルを削除）。解凍したフォルダの中にある「.exe」ファイルをダブルクリックして実行。指示に従って適当にOK、OKと押していく。特にいじらなくてOK。最後まで行ってインストール出来たっぽければ閉じる。もう一度デバイスマネージャーを確認して「ポート（COMとLPT）」→「COM？」が表示されていれば完了！元のCDM212364_Setupフォルダは削除してOKです。

最後にsample_position.pyの7行目あたりにあるdxl = dyna.Dynamixel("COM4",57600)の「COM4」の部分を各自の番号「COM?」に変更します。

ここまで出来たら実行してみましょう。
※ 右上の△マークをクリック。すると統合ターミナルで

---------------------------------
   Dynamixel READY TO MOVE  
---------------------------------

の表示がされることを確認して、統合ターミナル上のどこかを一度クリックして選択状態にしておく。
（早い話が、右上の△マークをクリックした後は「Dynamixel READY TO MOVE」の文字周辺を一度クリックする。）

そしてキーボードの右左矢印キーを押してみましょう。右を押せば時計回りにすこし動き逆も同じように動きます。また、その動く範囲はちょうど1回転です。

ではCtrl+Cで実行強制停止し、同様にsample_velocity.pyも実行して右左矢印キーを押してみましょう。今度は右を押すと時計回りに回転し、押せば押すほど早くなっていきます。逆に左を押すと減速しやがて反時計回りに回転します。

ではここまで出来たらCtrl+Cで実行強制停止して終了しましょう。

プログラムを理解しよう

それではプログラムの中身を見ていきましょう。たった30行程度のプログラムですね。

角度を制御するプログラム

1つ目のsample_position.pyのプログラムで、これは内部で位置（角度）の指定によって制御を行なっています。

import dynamixel_classes_for_windows as dyna
import kbhit
import time

try:
    dxl = dyna.Dynamixel("COM4",57600) #インスタンス化
    kb = kbhit.KBHit() #キーボード入力のクラス立ち上げ
    time.sleep(0.5) 
    dxl.set_mode_position(1) #位置（角度）制御モードに設定

はじめに必要なファイルをインポート（取り込み）したあと
クラスをインスタンス化させると同時に通信を行うために必要な数値を与えて内部で通信を確立させています。
次にこのDynamixelを位置制御モードに設定しています。Dynamixelではいくつかモードがあり、主に位置制御モード、拡張位置制御モード、速度制御モードがあります。

min_1=0
max_1=4095
dxl.set_min_max_position(1,min_1,max_1) #位置の上下限を設定
now_goal1=int(dxl.read_position(1)) #現在の位置を取得し目標値の初期値として代入
dxl.enable_torque(1) #トルクをオンにする（手で動かせなくなる）

そのあと、指示可能な位置の制限を設定しています。defaultは最小値0最大値4096(※)ですが、これを0~1024などにすると最大でも90°の範囲でしか動かなくなり例えばロボットの関節の回しすぎを防止するときに有効です。
次に目標値用の変数「now_goal1」に現在の値を初期値として代入しています。
最後にトルクをオンにしています。簡単に言うならDynamixelが内部で力を入れてモーターを回せる状態にする…といった感じでしょうか。
※　拡張位置制御モードでは360°以上の角度制御が可能でありこの限りではありません。

while 1:

while文のなかは無限ループしています。条件が１で常に真だからです。
このようにロボットプログラミングでははじめにセットアップ的な処理を書いて、その後のwhile 1:内でやりたい処理を書くということが一般的な書き方でよく使うので覚えておきましょう。

if kb.kbhit(): #キーボードの打鍵があったら（無いときはここで待ち状態になる）
  c = ord(kb.getch()) #キー入力を数値（番号）に変換
  if c==75: #L arrow #左矢印なら
    now_goal1=now_goal1+50  #目標値を50増やす
  elif c==77: #R arrow
    now_goal1=now_goal1-50
  now_goal1=max(min_1,min(now_goal1,max_1)) #数値的に上下限を超えないようにする
  dxl.write_position(1,now_goal1)  #Dynamixelに数値を書き込む

まずkb.kbhit()をして入力を待ちます。ここで何かキーボードの入力が入ると次行のkb.getch()でそのデータを受け取りそれをordで変換します。
…とはいえ、この部分はkbhit.pyの作者が提供しているサンプルコードに倣っているだけですので特に深く理解せずとも使えればOKです。
さて、今入力が右矢印キーだったとき「c」の値は75です（※ これは使用にのっていないので他のキーについて知りたい場合はprint(c)で見てみてください）。
ですのでc==75の場合、目標値を+50するという処理が行なわれます。
左矢印の場合はc==77で、目標値を-50します。
そして、加減算した目標値が min_1 < now_goal < max_1 となるようにするのがnow_goal1=max(min_1,min(now_goal1,max_1))です。max(a,b)はaとbのうち大きい値を返す関数、逆にmin(a,b)はaとbのうち小さい値を返す関数です。

ここまではあくまで数値計算です。
そして最後のdxl.write_position(1,now_goal1)でようやくDynamixelに目標値の書き込みを行ないます。
ここまでが主要部分です。

except KeyboardInterrupt: #Ctrl+Cが押されたら
    dxl.disable_torque(1) #トルクをオフにする（手で動かせるようになる）
    dxl.close_port() #ポートを切断する
    kb.set_normal_term() #キー入力に対する応答を通常時に戻す

ここはCtrl+Cが押されて強制停止するためのコード部分です。
Ctrl+Cが押されるとプログラム上で割り込んでexcept KeyboardInterrupt:に強制的に飛ばされこの中の部分が実行されます。

これが簡単な流れです。
角度制御の書き方をおさらいしましょう。

通信を確立する　→　角度制御用セットアップ　→　トルクをオンにする
無限ループ中
　値代入＆出力する
終了するとき
　トルクをオフにする→通信を切断する

回転方向が変化して回り続けるプログラム

次はsample_velocity.pyの中身を見てみましょう。こっちは速度を指示して制御をしています！

import dynamixel_classes_for_windows as dyna
import kbhit
import time

try:
    dxl = dyna.Dynamixel("COM?", 57600)  # インスタンス化
    kb = kbhit.KBHit()  # キーボード入力のクラス立ち上げ
    time.sleep(0.5)  # 通信が確立するまでちょっと待つ（待たなくても良いが高速すぎるとバッファが溢れ命令実行漏れが発生する）
    dxl.set_mode_velocity(1)  # 速度制御モードに設定
    max_1 = 250
    dxl.set_max_velocity(1, max_1)  # 速度の上限を設定
    dxl.enable_torque(1)  # トルクをオンにする（手で動かせなくなる）

    now_goal1 = 0

    print("---------------------------------")
    print("     Dynamixel READY TO MOVE   ")
    print("---------------------------------")

    while 1:
        if kb.kbhit():  # キーボードの打鍵があったら（無いときはここで待ち状態になる）
            c = ord(kb.getch())  # キー入力を数値（番号）に変換
            if c == 75:  # L arrow  #左矢印なら
                now_goal1 = now_goal1 + 50  # 目標値を50増やす
            elif c == 77:  # R arrow
                now_goal1 = now_goal1 - 50

            now_goal1 = max((-1) * max_1, min(now_goal1, max_1))  # 数値的に上下限を超えないようにする
            dxl.write_velocity(1, now_goal1)  # Dynamixelに数値を書き@込む

except KeyboardInterrupt:  # Ctrl+Cが押されたら
    dxl.disable_torque(1)  # トルクをオフにする（手で動かせるようになる）
    dxl.close_port()  # ポートを切断する
    kb.set_normal_term()  # キー入力に対する応答を通常時に戻す？）

わかるでしょうか？ほぼ角度制御の書き方と同じですね！
違うのはセットアップの関数と出力の関数、あとは代入値ですね。
Dynamixelを速度制御においては目標値が正の値のとき正回転、負の値のとき逆回転します。回転の速さは数値の大きさによって変化します。

プログラムの仕組みはコード上のコメント及び関数名にカーソルを重ねたときに見える関数の仕様を見ればおおよそ理解できるはずですのでここでは省略します。

アレンジしてみよう！

さて基本の流れがわかりましたね！
では以下のような動きをするプログラムを自力で書いてみましょう。

課題｜
上矢印キーを押す→1秒間MAX速度（250）で正回転する
下矢印キーを押す→3秒間MAXの半分の速度で逆回転する

※ヒント１：
上ボタンの「c」の値は72
下ボタンの「c」の値は80
if文を使って分岐させてみましょう。

※ヒント２：
time.sleep()メソッドを使用してみましょう。
time.sleep(1)とすれば１秒間待機する処理を命令できます。

できましたか？
ではプログラムの一例を示します。

import dynamixel_classes_for_windows as dyna
import kbhit
import time

try:
    dxl = dyna.Dynamixel("COM?", 57600)  # インスタンス化
    kb = kbhit.KBHit()  # キーボード入力のクラス立ち上げ
    time.sleep(0.5) 
    dxl.set_mode_velocity(1)  # 速度制御モードに設定
    max_1 = 250
    dxl.set_max_velocity(1, max_1)  # 速度の上限を設定
    dxl.enable_torque(1)  # トルクをオンにする（手で動かせなくなる）

    now_goal1 = 0

    print("---------------------------------")
    print("     Dynamixel READY TO MOVE   ")
    print("---------------------------------")

    while 1:
        if kb.kbhit():  # キーボードの打鍵があったら（無いときはここで待ち状態になる）
            c = ord(kb.getch())  # キー入力を数値（番号）に変換
            if c == 72:  # UP arrow 
              now_goal1=max_1
              dxl.write_velocity(1, now_goal1)
              time.sleep(1) #1秒待つ
              now_goal1=0
              dxl.write_velocity(1, now_goal1)
              
            elif c == 80:  # DOWN arrow
              now_goal1=(-1)*max_1/2
              dxl.write_velocity(1, now_goal1)
              time.sleep(2) #2秒待つ
              now_goal1=0
              dxl.write_velocity(1, now_goal1)

except KeyboardInterrupt:  # Ctrl+Cが押されたら
    dxl.disable_torque(1)  # トルクをオフにする（手で動かせるようになる）
    dxl.close_port()  # ポートを切断する
    kb.set_normal_term()  # キー入力に対する応答を通常時に戻す？）

うまくできましたか？
もちろん、今完璧である必要はありません！
コツコツと引き続きプログラミング経験を重ねていきましょう！

ところで、Dynamixelとは

Dynamixelはスマートアクチュエーターであり、種類でいえばシリアルサーボモーターの１つです。サーボモータとはモーターの中でも回転軸の位置を指定できる角度制御に特化したモーターのことで、とくに外部の機器とシリアル通信（通信の一種）を行うことでそれが操作できるものをシリアルサーボモーターといいます。
DYNAMIXEL | ROBOTIS e-Shop

おわりに

お疲れさまでした！
とうとうロボットのプログラミングをやっている感じになってきましたね。
次回もお楽しみに！

講座シリーズ｜
超小型PCラジコンカーで学ぶPythonプログラミング！
（次回）> 3. Linuxに挑戦！PCをコマンド遠隔操作してみよう！
ミニロボットアームで学ぶPythonプログラミング！
（次回）> 3. 実践！ミニロボットアームを動かそう！
< 大会多忙により記事製作中断中 >

参考サイト：
DYNAMIXEL | ROBOTIS e-Shop
Besttechnology - DYNAMIXELベーシックチュートリアル [DYNAMIXEL basic tutorial] - ナレッジベース