KRSサーボモータを動かそう(Python編)

Posted on 2021.12.20 in

KRSサーボは、一般的なPWMでの制御の他にシリアル通信で送受信したコマンドにより制御することが可能です。この記事では、PCにUSB経由でサーボを接続し、Pythonで作成したプログラムからKRSサーボを動かす方法をご紹介します。

サンプルのPythonのプログラムは、処理の内容がわかりやすいようになるべくシンプルに書きました。Pythonで開発できる環境があれば、Windowsのみならず、LinuxでもKRSサーボを制御できるようになります。この記事を参考にぜひKRSサーボをご利用ください。

B3Mサーボでも同じようにPythonで動かす記事を公開しました。『B3Mサーボモータを動かそう(Python編(1))』をご参照ください。

【過去の記事】

　　シリアルサーボ制御方法（１）　回路編

　　シリアルサーボ制御方法（２）　ICS編

　　シリアルサーボ制御方法（３）　ソフト編

　　シリアルサーボ制御方法（４）　PCから直接制御編（その１）

　　シリアルサーボ制御方法（５）　PCから直接制御編（その２）

■KRSサーボ

KRSサーボ商品ページカテゴリー

トルクやスピード、ケースサイズなどの違いにより多彩な商品をラインアップしています。ケースサイズごとにシリーズが分かれています。同じシリーズであれば、ホーンやケーブル、フレームなどのパーツを共有することが可能です。

KRSサーボは、ICSという近藤科学独自の通信プロトコルを採用しています。現在は、ICS3.5と3.6のサーボを販売していますが、3.6は3.5の上位互換ですので混在して使用することができます。ICS3.5と3.6の違いについては、下記の記事をご参照ください。

テクニカルガイド『ICS3.5/3.6解説』

また、ICS3.5/3.6の通信や機能に関する詳しい情報は、下記のソフトウェアマニュアルに掲載されています。

『ICS3.5 / 3.6ソフトウェアマニュアル（コマンドリファレンス）』

KRSサーボは、対応電圧が2種類あります。HVは9～12V、LVは6～7.4V対応です。対応電圧が異なるサーボは混在できませんのでご注意ください。HVサーボの場合は、商品名にHVの記載があります。LVは無記名です。

■必要な製品

KRSサーボを制御するために必要な製品は、下記の通りです。

・電源
サーボの対応電圧に合った電源をご用意ください。お勧めの電源は各サーボの商品ページに掲載されています。

・接続ケーブル
サーボに付属しています。ケーブルの種類によっては、長さの異なるケーブルをラインアップしています。サーボの搭載個所によりちょうどよい長さのケーブルをご利用ください。

・Dual USBアダプターHS
PCとサーボ間をUSB経由で接続するためのアダプターです。サーボの通信に必要な各種ケーブルが付属しています。

■開発環境

Pythonの開発環境を用意してください。この記事ではAnacondaを利用しました。

※Anacondaのインストール、ご利用方法はこの記事ではご紹介していません。別途お調べになりご準備をお願いします。

また、USB経由でデバイスと通信するためにpySerialのライブラリをインストールする必要があります。Anacondaの場合は、Anacondaのコンソールから下記のコマンドを実行してインストールすることができます。

pip install pyserial

1	pip install pyserial

正常にインストールできたかを確認するために、pySerialのバージョンを確認してみます。エディターに下記にプログラムを書いて実行してください。

import serial

print(serial.__version__)

import serial

print(serial.__version__)

コンソールにバージョン情報が表示されましたらインストールが成功しています。

■KOドライバをインストール

Dual USBアダプターHSをPCに認識させるためにKOドライバをインストールします。下記のページからファイル一式をダウンロードしてください。Windowsの場合は、ダウンロードしたフォルダ内にある付属のマニュアルをご参照ください。

『KO Driver 2015』

Linux系のOSの場合は、下記の記事をご参照ください。弊社ではRaspberryPiのRaspbianOSにて動作を確認しました。

『弊社USBアダプタをLinuxで使うには(2019版)』

■プログラム解説

【KRSサーボの設定】

以下のサンプルプログラムでは、KRSサーボの設定は工場出荷状態のID0、通信速度115200bps、個数は1個とします。

ID番号を変更することで複数のサーボをデイジーチェーン接続して制御することができますが、通信速度はすべて同じである必要があります。

サンプルプログラム「KRS_Python_Sample_1」に用意されている関数一覧

下記のリンクからサンプルプログラムをダウンロードすることができます。このプログラムには、以下の関数が用意されています。

サンプルプログラム＜ダウンロード＞

krs_setPos_CMD(servo_id, pos)
サーボのID番号とポジション（角度）を指定してサーボを動かすための関数です

krs_setFree_CMD(servo_id)
指定したIDのサーボを脱力することができます。ICSの機能としてポジションデータを0で送信すると脱力します。この関数内ではsetPos関数をそのまま使用し、ポジション0を送信しています。

krs_getValue_CMD(servo_id, sc)
サーボのパラメータを読み出すことができます。サブコマンド（sc）により読み出すパラメータを指定することができます。

krs_getPos36_CMD(servo_id)
ICS3.6用の現在位置を取得するための関数です。

krs_getPos35_CMD(servo_id)
ICS3.5用の現在位置を取得するための関数です。

krs_setValue(servo_id, sc, Value)
サーボのストレッチ、スピード、電流制限値、温度制限値を書き換えることができます。サブコマンド（sc）により書き込むパラメータを指定することができます。

krs_getID_CMD()
サーボのID番号を取得するための関数です。
※ID番号を読み出す場合は、必ずホストとサーボを1対1で接続してください。複数台つないだ状態では正常な値を取得できません。

以下の解説は、代表的なsetPos関数と、ICSバージョンにより使い分けが必要なgetPos関数について解説します。その他の関数は基本的にsetPosと同じ構造ですので、詳しくはサンプルプログラムをご参照ください。

COMを開く

まずは、PCとKRSサーボの通信を開始するために、COMを開きます。

import serial
import time

krs = serial.Serial('COM3', baudrate=115200, parity=serial.PARITY_EVEN, timeout=0.5)

import serial

import time

krs = serial.Serial('COM3', baudrate=115200, parity=serial.PARITY_EVEN, timeout=0.5)

最初に上記の通りimportで「serial」をインポートします。「time」は後ほど時間を待つのにsleep関数を使うので、インポートしておいてください。

「serial.Serial()」でCOMポートを開くことができます。

引数の「COM3」は、USBアダプタがつながれているCOM番号です。Windowsのデバイスマネージャーから調べることができます。詳しくは、KOドライバ付属のマニュアルを参照してください。

Linuxでポートを指定する場合は、'/dev/ttyAMA0'と表記します。※ポートの表記方法は環境により異なる場合があります。

「baudrate」はPC、サーボ間の通信速度です。制御するKRSサーボの通信速度を指定してください。KRSサーボは工場出荷状態で「115200bps」に設定されています。

「parity」でパリティを指定します。KRSサーボは「偶数パリティ」なので「serial.PARITY_EVEN」を指定します。

「timeout」は、デバイスからの返事がない場合の待ち時間です。KRSサーボが接続されていない、または電源が入っていないなど、通信ができない状態の時にどれくらいの時間を待つのか指定できます。今回は0.5秒に設定しました。

以上を「krs」という名称でインスタンス化しました。以降は「krs」でpySerialの各機能を使用できるようになります。

krs.close()

1	krs.close()

プログラムの最後にkrs.close()を書くことでCOMを閉じることができます。

ポジションコマンドを作成する（setPos関数）

KRSサーボは、ポジションコマンドにより対象となるサーボのID番号とポジション（角度）を送信することでサーボを動かすことができます。

【ポジションコマンド構成】

詳細は、『ICS3.5 / 3.6ソフトウェアマニュアル（コマンドリファレンス）』のP.13をご参照ください。

def krs_setPos_CMD(servo_id, pos):

    txCmd = [0x80 | servo_id, #ポジションコマンド0ｘ80にＩＤ番号を足し合わせてヘッダにします
             pos >> 7 & 0x7f, #ポジションデータを2バイトに分けます
             pos & 0x7f]

    #コマンドを送信
    krs.write(txCmd)

    #サーボからの返事を受け取る
    rxCmd = krs.read(6)

    #もしリスト何になにも入っていなかったら正常に受信できていないと判断
    if len(rxCmd) == 0:
        return False

    #問題なければ返事を返す
    else:
        #受け取った2Byteの現在位置を1バイトのデータにまとめて戻り値にします
        #※この現在位置は、サーボがポジションを受け取って動き出す前の現在位置です
        return True, (rxCmd[4] << 7) + rxCmd[5])

def krs_setPos_CMD(servo_id, pos):

txCmd = [0x80 | servo_id, #ポジションコマンド0ｘ80にＩＤ番号を足し合わせてヘッダにします

pos >> 7 & 0x7f, #ポジションデータを2バイトに分けます

pos & 0x7f]

#コマンドを送信

krs.write(txCmd)

#サーボからの返事を受け取る

rxCmd = krs.read(6)

#もしリスト何になにも入っていなかったら正常に受信できていないと判断

if len(rxCmd) == 0:

return False

#問題なければ返事を返す

else:

#受け取った2Byteの現在位置を1バイトのデータにまとめて戻り値にします

#※この現在位置は、サーボがポジションを受け取って動き出す前の現在位置です

return True, (rxCmd[4] << 7) + rxCmd[5])

txCmd = [0x80 | servo_id,
pos >> 7 & 0x7f,
pos & 0x7f]

ポジションコマンドの送信コマンドは、３バイトのデータで構成されています。

1バイト目は、コマンドの種類とID番号を指定します。0x80 | servo_idは、機能としてポジションコマンドを指定するための0x80とサーボIDを足し合わせる処理です。これを送信コマンドのヘッダとしています。

2バイト目と3バイト目は、ポジションデータの上位と下位を格納します。ICSのルールとして、送信コマンドのヘッダ以外は最上位ビット（MSB）を0にする必要があります（一部例外あり）。そのため、7ビットのみシフトし、0x7f（0b01111111）でマスクしています。

ポジションデータは3500～7500～11500の範囲で送信することにより-135°～0～135°に動作するように指定できます。7500が、サーボの中心位置です。

サーボにポジションデータとして0を送信すると脱力した状態（フリー）になります。

krs.write(txCmd)

rxCmd = krs.read(6)

以上で用意された送信コマンドをpySerialのwrite関数で送信します。KRSサーボがコマンドを受け取ると、返信データを返してきますので、pySerialのread関数でデータを受け取ります。

Dual USBアダプターHSは送ったデータがそのままループバックで返ってきます。ポジションコマンドのサーボモータからの返信は3バイトです。そのため、送信コマンド数(3バイト)＋サーボモータの返信数(3バイト)の合計6バイトを受け取ります。read関数の引数の(6)は、データを受けとるデータ数で、ポジションコマンドの場合は6を指定しました。

return True, (rxCmd[4] << 7) + rxCmd[5]

サーボからの返事には、現在位置が含まれています。送信時と同じく、返ってきた現在位置も2バイトデータですので上記の処理により1つのデータにまとめる必要があります。

※返事として受け取った現在位置は、サーボがポジションコマンドを受け取った時点のデータであり、動作開始前のものです。動作後の現在位置を取得する方法は後述のgetPos関数を使用してください。

サーボの現在位置を取得する（getPos関数）

サーボの現在位置を取得する方法は、ICS3.5と3.6で異なります。

・ICS3.6の場合

ICS3.6は現在位置を取得するコマンドが実装されていますので、読み出しコマンドでシンプルに読むことができます。

【読み出しコマンド構成（角度）】

詳細は、『ICS3.5 / 3.6ソフトウェアマニュアル（コマンドリファレンス）』のP.16をご参照ください。

def krs_getPos36_CMD(servo_id):

    txCmd = [0xA0 | servo_id, #読み出しコマンド0xA0にＩＤ番号を足し合わせてヘッダにします
             0x05]            #sc（サブコマンド）により、現在位置を読み出します

    #コマンドを送信
    krs.write(txCmd)

    #サーボからの返事を受け取る
    rxCmd = krs.read(6)

    #もしリスト何になにも入っていなかったら正常に受信できていないと判断
    if len(rxCmd) == 0:
        return False, 0

    #問題なければ返事を返す
    else:

        #読み出した2バイトのデータを１つのデータにまとめて戻り値とします
        return True, (rxCmd[4] << 7) + rxCmd[5]

def krs_getPos36_CMD(servo_id):

txCmd = [0xA0 | servo_id, #読み出しコマンド0xA0にＩＤ番号を足し合わせてヘッダにします

0x05] #sc（サブコマンド）により、現在位置を読み出します

#コマンドを送信

krs.write(txCmd)

#サーボからの返事を受け取る

rxCmd = krs.read(6)

#もしリスト何になにも入っていなかったら正常に受信できていないと判断

if len(rxCmd) == 0:

return False, 0

#問題なければ返事を返す

else:

#読み出した2バイトのデータを１つのデータにまとめて戻り値とします

return True, (rxCmd[4] << 7) + rxCmd[5]

txCmd = [0xA0 | servo_id, #読み出しコマンド0xA0にＩＤ番号を足し合わせてヘッダにします 0x05] #sc（サブコマンド）により、現在位置を読み出します

読み出しコマンドの送信コマンドは2バイトのデータで構成されています。

1バイト目は、ポジションコマンドと同じくコマンドの種類とID番号を指定します。読み出しコマンドは、0xA0を指定し、ID番号と足し合わせています。

2バイト目は、サブコマンドです。サブコマンドにより読み出すパラメータを指定できます。サブコマンドの種類は下記の通りです。

　0x00：EEPROM　　サーボのEEPROMに格納されたデータをすべて読み出します。
　0x01：ストレッチ　現在設定されているのストレッチの値を取得します。
　0x02：スピード　　現在設定されているのスピードの値を取得します。
　0x03：電流値　　　現在の電流値を取得します。
　0x04：温度　　　　現在のサーボ内の温度を取得します。
　0x05：現在位置　　現在のサーボの角度を取得します。

現在位置を取得するサブコマンドは0x05ですので、送信コマンドに0x05を格納しました。

この送信コマンドをポジションコマンドと同じ方法で送信し、サーボからの返事を受け取ります。Dual USBアダプターHSを使用した場合、ループバック込みで処理すると、受け取ったデータの5バイト目と6バイト目に現在位置が格納されていますので、下記の処理で1つのデータにまとめ、戻り値にしています。

return True, (rxCmd[4] << 7) + rxCmd[5]

retuenのTrueは、通信が正常に行われたことを表すために戻り値に入れました。

・ICS3.5の場合

def krs_getPos35_CMD(servo_id):

    #ポジションコマンドの戻り値で現在位置を取得するため、ポジション0でサーボから戻り値を取得する
    bl, Value = krs_setPos_CMD(servo_id, 0)

    #フリー状態のサーボに取得したポジションと同じポジションを送信し、トルクオン状態に戻します
    bl, Value = krs_setPos_CMD(servo_id, Value)

    return bl, Value

def krs_getPos35_CMD(servo_id):

#ポジションコマンドの戻り値で現在位置を取得するため、ポジション0でサーボから戻り値を取得する

bl, Value = krs_setPos_CMD(servo_id, 0)

#フリー状態のサーボに取得したポジションと同じポジションを送信し、トルクオン状態に戻します

bl, Value = krs_setPos_CMD(servo_id, Value)

return bl, Value

ICS3.5の場合は、読み出しコマンドで現在位置を取得することができませんので、ポジションコマンドを送信した時に返事で返ってくる現在位置を利用します。

サンプルにあるkrs_getPos35_CMD()の内容は、ポジションコマンドでポジション0を送信し、脱力した状態で現在位置を取得しています。その後、取得したポジションデータと同じポジションをポジションコマンドで送信し、サーボをトルクオンの状態にしています。それぞれの指示は、setPos関数により実行しています。

setPos関数、getPos関数を使ってみる

STRETCH = 0x01     #ストレッチ（サーボの保持力を指定するパラメータ）
SPEED = 0x02       #スピード　※スピードのパラメータを下げるとトルクも下がります
CURRENT = 0x03     #電流値（書き込みの時は電流制限値）
TEMPERATURE = 0x04 #温度（書き込みの時は温度制限値）


#COMポートを開く
krs = serial.Serial('COM3', baudrate=115200, parity=serial.PARITY_EVEN, timeout=0.5)


#IDで指定したサーボにポジション（角度）を指定し動作します
#動作角指定範囲
#POS:3500=7500~11500
#角度:-135°～0°～135°
bl, reData = krs_setPos_CMD(0, 11500) #135°へ移動
time.sleep(2.0) #サーボが目標位置に到着するまで少し待ちます

bl, reData = krs_setPos_CMD(0, 3500) #-135°へ移動
time.sleep(2.0)

bl, reData = krs_setPos_CMD(0, 7500) #0°（中心位置/ニュートラル位置）へ移動
time.sleep(2.0)


#サーボの現在位置を取得します（ICS3.6の場合）
bl, reData = krs_getPos36_CMD(0)
print("POS36", reData)

#サーボの現在位置を取得します（ICS3.5の場合）
bl, reData = krs_getPos35_CMD(0)
print("POS35", reData)


#ポートを閉じる
krs.close()

STRETCH = 0x01 #ストレッチ（サーボの保持力を指定するパラメータ）

SPEED = 0x02 #スピード　※スピードのパラメータを下げるとトルクも下がります

CURRENT = 0x03 #電流値（書き込みの時は電流制限値）

TEMPERATURE = 0x04 #温度（書き込みの時は温度制限値）

#COMポートを開く

krs = serial.Serial('COM3', baudrate=115200, parity=serial.PARITY_EVEN, timeout=0.5)

#IDで指定したサーボにポジション（角度）を指定し動作します

#動作角指定範囲

#POS:3500=7500~11500

#角度:-135°～0°～135°

bl, reData = krs_setPos_CMD(0, 11500) #135°へ移動

time.sleep(2.0) #サーボが目標位置に到着するまで少し待ちます

bl, reData = krs_setPos_CMD(0, 3500) #-135°へ移動

time.sleep(2.0)

bl, reData = krs_setPos_CMD(0, 7500) #0°（中心位置/ニュートラル位置）へ移動

time.sleep(2.0)

#サーボの現在位置を取得します（ICS3.6の場合）

bl, reData = krs_getPos36_CMD(0)

print("POS36", reData)

#サーボの現在位置を取得します（ICS3.5の場合）

bl, reData = krs_getPos35_CMD(0)

print("POS35", reData)

#ポートを閉じる

krs.close()

krs_setPos_CMD関数の引数に、サーボのID番号とポジション（角度）を渡します。該当するIDのサーボは、受け取ったポジションの位置へ動作します。サーボが指定した位置に到着する前に次のコマンドを受け取ってしまうと目標位置に到着できませんので、time.sleep()で少し処理を止めます。このサンプルの通り実行すると、左に135°、右に135°、最後に中央に移動します。

サーボ本体のID番号と、setPos関数のIDを変更すれば、複数のサーボをデイジーチェン接続でつないで制御することができます。

例として、動作が終わった後に現在位置を読み込むプログラムを入れました。お手持ちのICSバージョンに合わせてご利用ください。

※モータの個体差、ギヤのバックラッシュなどの違いにより、目標位置に対して現在位置に多少の誤差が出る場合があります。

KRSサーボをPythonで制御する解説は以上です。B3Mサーボに比べると機能は少ないですが、シンプルなところが初心者でも使いやすいポイントだと思います。また、低価格なサーボから高出力なサーボまでラインアップが揃っていますので、初めはKRS-3301のような扱いやすいサーボから始め、後に90kgf.cmを出力するKRS-9004HV搭載機など大型なロボットへも段階的に進めることができます。もちろん、同じICSの通信プロトコルを採用しているため、同じプログラムで動作します。

ぜひともKRSサーボをご利用ください。

【補足】

・ICS3.5/3.6 Manager software

こちらのソフトでIDの設定や通信速度の変更、スピードやストレッチの変更などICS3.5、3.6対応サーボの各種設定を変更することができます。

・無限回転モード

上記のマネージャソフトで無限回転モードに切り替えると、車輪のようにサーボの軸が回転するようになります。回転速度、回転方向を指定する場合は、setPos関数をそのまま使用します。7500で停止、7499より値を小さくすると左回転に速くなっていき、7501より大きくすると右回転に速くなっていきます。

この状態で現在位置を取得することもできますが、KRSサーボに搭載している角度センサの読める範囲は270°までですので、センサの反応しない90°の範囲をでは正常なデータを取得することができませんので、予めご了承ください。

・シリアルモード、PWMモードの切り替え

ICS3.5/3.6のKRSサーボは、シリアルモードとPWMモード両方に対応しています。モードの切り替えは、電源投入後の信号線のH/Lレベルにより決められます。電源投入後、500msの間Hレベルだとシリアルモード、LレベルだとPWMモードです。Dual USBアダプターHSで通信する場合は、必ずシリアルモードになりますので、今回はモード切替を気にする必要はありません。もし、お手持ちのマイコンボードなどでご利用いただく場合は、電源投入後のH/Lレベルにご注意ください。

キーワード検索