JP200(PoC)

Overview

ルネサスエレクトロニクス社(以後renesas)のコンポーネントを使用したブラシ付きDCモータを簡易的に制御するPoC(概念実証)ベンチ。
外観は既成のサーボモータと酷似し、簡単なテキストコマンドにより角度・速度・電流の各制御指令を行う。

Specification

Fundamental spec.

機構はKONDO KRS-2500シリーズと互換性があるので、それ用に提供されるパーツが流用可能。

Layout

E192_LAYOUT.pdf

Schematic

E192_SCH.pdf

Terminals

CN1, CN2

• 電源並びに通信用信号ラインを接続

  Pats Name	JST Parts Number
  基板用ヘッダー	B3B-ZR
  ハウジング	ZHR-3
  ターミナル	SZH-002T-P0.5

  

  端子番号	信号名
  1	Signal	シングルエンドのシリアル通信信号 双方向
  2	VDD	電源のプラス側
  3	GND	電源のマイナス側 シリアル通信信号の基準電位と共有

LED

• 緑色LED
• アプリケーションにてプログラマブルに点灯・消灯
• 電流制限抵抗を介してMPUのGPIOにソース接続

CN3

• パッドのみ
• USBにかかる信号を接続
  

CN4

• ランドのみ
• SWDにかかる信号を接続
  

Details of each function

Bootloader

初期ブートローダ
https://www.besttechnology.co.jp/downloa​d/JP200_bootloader_20230429.7z

• fsp4.4

2023/9/23更新
https://www.besttechnology.co.jp/downloa​d/JP200_bootloader_20230923.7z

• fsp4.6
• ボーレートを3M[bps]から1M[bps]へ変更
• 一部のホスト側のI/Fにおいてバスの切り替えタイミングがブートローダの送信とコンフリクトし文字化けする事があったため、送信前に100usの遅延を挿入

本ブートローダはシングルチップモードで動作するアプリケーションプログラムであり、MPUが持つブートプログラムとは異なる。
主たる目的はJP200が備えている半二重シリアルI/Fを介して任意のプログラムを転送し内蔵フラッシュメモリに書き込む事と、そのプログラムの実行手段を提供する事にある。

本ブートローダから実行できるプログラムにはいくつか制約がある。MPUが想定しているスタートアドレスが0x0番地なのに対し、0x0番地は本ブートローダ本体が書き込まれているため、0x00010000をスタートアドレスに設定する必要がある。またオフセットしたアドレスから起動している前提で各レジスタが初期化されている必要がある。
次に本ブートローダのダウンロードがサポートするファイルは、メモリ上に展開した状態をそのままバイナリデータ化したもののみ。HEX・MOT・SREC・ELFといったファイルフォーマットには対応していない。
e2studioでプロジェクトを作成する際の要点はこちらを参考に。

Boot conditions

コールドスタートやホットスタート、ユーザープログラムの状態による分岐を以下に記載する。

• 実行可能なユーザプログラムが書き込み済み

  電源を印加するとブートローダはそのユーザプログラムにジャンプ。
  I/Fから'!'を連続で受信させながら電源を印加するとブートローダはコマンドモードで待機。

• ユーザプログラムが消去もしくは実行不可

  電源を印加するとブートローダはコマンドモードで待機。

• ユーザプログラム内でソフトウェアリセット発行

  ソフトウェアリセット後にブートローダはコマンドモードで待機。

Transfer and execution of user programs

ユーザプログラムの転送にはレガシーなXMODEM/1kプロトコルのみに対応1し、I/Fの設定は1Mbps 8N1を要求する。XMODEMが半二重を前提としているので、ホスト側のターミナルソフトウェアはI/Fが半二重であることを特段意識する事はない。

ユーザープログラムの転送手順は以下の通り。
まず'e'を送信してフラッシュメモリの消去を起動する。

RA4M2 BL1.2>e(y/n)

ここで'y'を送信すると実際に消去される。

RA4M2 BL1.2>e(y/n)y
OK
RA4M2 BL1.2>

次に'w'を送信して転送モードを起動する。

RA4M2 BL1.2>w(y/n)

ここでyを入力したら、すかさず転送プロトコルに従って任意のバイナリファイルの送信を開始させる。成功すれば<OK>、失敗すると<NG>と表示される。

RA4M2 BL1.2>w(y/n)y
OK
RA4M2 BL1.2>

'g'を送信してフラッシュメモリに書き込まれたユーザプログラムを実行する。

RA4M2 BL1.2>g

これらの一連の操作をTeraTermで行う際のマクロを紹介しておく。

upload_bin.ttl

Firmware

初期ファームウェア
https://www.besttechnology.co.jp/downloa​d/JP200_20230721.7z

• fsp4.5

2023/9/18更新
https://www.besttechnology.co.jp/downloa​d/JP200_20230918.7z

• fsp4.6
• 符号付32ビットデータに-2147483648が設定されている場合、それを読み出すとパケットデータが異常になるのを修正
• 各ゲインの最大値を10000に変更
• パワーサイクル時の不揮発データの適用を活性化
• 単位の無かった速度を回転数[x1000 rot/sec]に変更
• 偏差によるPWM制限機能としてOffsetAngle, AngleFlexibility, CurrentFlexibilityを追加
• 指定時間で最終指令値から目標値へ遷移させる機能としてTransitionMode, TransitionTimeを追加
  それに伴いTA,TV,TCコマンドにTransitionModeとTransitionTimeの指定を追加
• 角度制御時の動作可能範囲としてAngleLimitを追加
• I2Cの処理が破綻するために最適化を抑止していたが、プロジェクトの最適化レベルを-O3にし、最適化による影響を各ソースでコントロール
• 文字列処理の大半を標準ライブラリに置き換え
• パケット処理を見直し応答時間を改善
• 多少オーバフロー意識したコードに修正
  それに伴い各所のバッファをstatic化しアロケート時のオーバヘッドを軽減
• エンコーダエラー時に制御を停止させ、LEDを点滅させる
  リカバリはパワーサイクルのみ

2023/9/26更新
https://www.besttechnology.co.jp/downloa​d/JP200_20230926.7z

• fsp4.6
• デフォルトボーレートを1M[bps]に変更
• 応答遅延としてReturnDelayTimeを追加
• 監視機能としてMonInfo[0]~MonInfo[4]を追加
• 監視機能の追加に伴いControlDurationTime, NoCommTime, AngleViation, VelocityViation, CurrentViationを追加
• コントロールテーブルのサイズが更新されたため、本ファームウェアを適用すると不揮発領域は初期化される

2023/10/11更新
https://www.besttechnology.co.jp/downloa​d/JP200_20231011.7z

• fsp4.6
• 各制御にフィードフォワードを追加
  それに伴いSG0~2コマンドの引数を追加
• 各制御ゲインの上限値を30000に変更
• 各制御における指令値に加算するオフセット指令値を追加
• 日替わりで更新している事もあり、ひとまずVERコマンドを追加
• TA/TV/TC/SGコマンドの数値切り出しに失敗するようなので処理方法を変更
• RRコマンドで温度の更新がなされないのを修正
• 通信が復帰しないケースが散見されたので、暫定的にuartのコールバック中にエラー処理を追加
• コントロールテーブルのサイズ及びアイテムのインデックスが更新されたため、本ファームウェアを適用すると不揮発領域は初期化される

2023/10/30更新
https://www.besttechnology.co.jp/downloa​d/JP200_20231030.7z

• fsp5.0
• すっかり失念していたCRCの付与に対応
• 非接触エンコーダとの通信に使っているCRCルーチンを分離
• 少し攻め気味だった速度制御のゲインを低めに変更
• 制御モードに静止を追加
• 大きめのTransitionTimeと目標値を指定すると容易にオーバーフローするので計算は64bitに変更
• 角度制御以外から角度制御に切り替えた直後に遷移時間を指定して運転すると開始角度が初期化されないのを修正
• 同梱のjp200_test.exeを更新

General features

一般的な算数通りに演算してモータをフィードバック制御しているのみで、特筆すべき所は無い。唯一あるとすればシリアルサーボの類にありがちなバイナリベースの通信プロトコルを一切使わず、全て目視で判断できるテキストのみの電文で処理している点。なおI/Fは1ワイヤの半二重通信を前提とし、昨今のデバイスの能力に任せてメガbpsでの通信を基本とする。
ファームウェアはe2studioを用いて作られており、renesasから提供されるfspを一通り使用した。開発当初はSWD端子にSEGGER社のJ-Linkを接続してデバッグを行っていたが、アプリケーションがテキストによる通信を行うもののため、最終的にはブートローダを介したファームウェアの書き換えとprintfデバッグを併用した。

なおソース単位で機能が囲われているかのように見えるかも知れないが、シングルソースだったものを後にそれっぽく分割した程度である。またC++でクラス化していた時分もあったが、積極的に使い始めた途端に破綻したのでCに戻した経緯がある。

Communication protocol

ホストもしくはサーボのパケットである事を区別するため、それぞれ不等号の「<>」と括弧の「()」のヘッダとフッダで囲われた範囲を有効なパケットとする。このヘッダとフッダに挟まれた中に必要な命令や応答を記述するが、使用できるのはアスキー文字の「0」~「9」, 「A」~「Z」, 「a」~「z」,「#」,「=」, 「+」, 「-」, 「;」, 「:」, 「,」のみで、それ以外の文字コードが括弧内に含まれているとパケットごと破棄する。
命令や応答の詳細は後述するとして、IDが各々1と2に設定された2つのサーボにホストからパケットを送信し、2つのサーボから応答が返った場合の例を示す。

ホスト:<#1EX=1TA=-300#2EX=2TV=300>
サーボ:(#1EX=OKTA=OK)(#2EX=OKTV=OK)

全てが想定通りな場合、2つのサーボはホストのパケットに記述されたID順にパケットを返信する。またホストのパケットに存在しないIDが含まれている場合は、それ以後のIDを持つサーボからの応答は返らない。

なおホストのパケットを受け取ったサーボは必ずパケットを返信するため、ホストはその返信されたパケットを受信しなくてはならない。返信のパケットの受信を待たずしてホストが新たなパケットを送信すると、ホストとサーボのパケット同士がコンフリクトし半二重通信が破綻する。また何らかの要因でサーボから応答のパケットを受信できなかった場合に備え、タイムアウト等で受信を諦める処理が必要である。
複数軸の応答となると、受信を終えるまでの時間はその軸数分かかり、条件によってはホストの指令周期を担保できなくなる場合がある。応答を必要としないのであれば、ホストのパケットを括る括弧を「[]」に変更して送信する事で、サーボは応答を返さなくなる。

更にホストのパケットを括る括弧を「{}」に変更した場合は、末尾の括弧の直前に「:」と10進数でCRC8を付与する必要がある。そのパケットを正常に受信したサーボの応答も同様に「:」とCRC8が付与される。ここで使用したCRC8の詳細はプロジェクトのソースとPythonの送受信クラスを参考にしてもらうとして、これによりテキストかつシングルエンドによる脆弱な通信環境が僅かながら堅牢になる。

ホスト:{#0EX=1TA=9000TP=1000#1EX=1TA=9000TP=1000:170}
サーボ:(#0EX=OKTA=OKTP=OK:70)(#1EX=OKTA=OKTP=OK:195)

Command list

以下の「コマンド」に記述された「<」「>」「[」「]」はコマンドの構文の説明としての記号であり、「<>」内は必須、「[]」内はオプションを説明しているもので、実際にパケット中にそれらの記号を記述してはならない。また数値は全て10進数表記である。

Control table

TAやTVといったコマンドは、実際には次に示すコントロールテーブルへのアクセスに変換される。コントロールテーブルへの直接的なアクセスにはWRやRRコマンドを使用する。

ID

個体識別番号。
0~254の範囲。

Baudrate

通信速度[bps]のインデックス。

OffsetAngle

角度のオフセット値[x100 deg]。
内部ではセンサから得られた角度からオフセット値を減じて使用される。

LED

LEDの点灯・消灯。
監視機能の条件一致時のLED点滅が優先される。

ReturnDelayTime

応答パケットの送信開始前に挿入する遅延時間[ms]。
実際にはパケット処理に時間を要し、遅延時間にその処理時間を加えた時間が経過した後に応答する。

System Status

システムステータス。

AngleFlexibility

メインループが角度制御の時、指令角度と現在角度の偏差によってPWM出力の制限を行う。
n_compliance[x100 deg]<=n_dedzone[x100 deg]<=p_deadzone[x100 deg]<=p_compliance[x100 deg]とする事。

deadzoneの間は内部の制御量がリセットされる。

CurrentFlexibility

メインループが電流制御の時、指令電流と現在電流の偏差によってPWM出力の制限を行う。
n_compliance[mA]<=n_dedzone[mA]<=p_deadz​one[mA]<=p_compliance[mA]とする事。

deadzoneの間は制御量がリセットされる。

AngleLimit

角度制御時の目標角度を内部的にmin～maxの範囲に飽和させる。
この範囲を超えた目標角度を指令してもエラーにはならない。

AngleCtrlGain, VelocityCtrlGain, CurrentCtrlGain

各制御対象におけるPID制御の各ゲイン(x1000)とフィードフォワードゲイン(x1000)、離散積分時間[ms]。
各ゲインは1/1000した上で演算に使用される。

delta_t は各制御対象の固定サンプリング時間のため変更は推奨しない。
ブロック図は敢えて示さないが、単純なPID演算とカスケード及びフィードフォワード。演算上のオーバーフローなどは一切考慮していないので、極端な数値指定は禁物。

TransitionMode

TransitionTimeが1以上の時に目標角度へ遷移するまでの軌跡を指定。

TransitionTime

最後の角度指令値から目標角度へ遷移するまでの時間[ms]。
その際の軌跡はTansitionModeによる。

Excitation

制御モード。
先頭に記述したものがメジャーループ、それ以降に複数記述されているものがマイナーループのカスケード制御となる。PWMについてはいずれの場合においても最終出力段のリミッタとして機能する。

TargetAngle

目標角度[x100 deg]。
数値を増加させるとサーボの出力軸は時計回りに回転する。 制御時の目標角度はAngleLimitの上下限値に制限される。
動作範囲が限られた機構に適用する場合は、角度センサの中央値である18000(180[deg])を動作範囲の中央位置近傍とする事。

TargetVelocity

目標回転数[x1000 rot/sec]。
数値を増加させるとサーボの出力軸は時計回りに回転する。

TargetCurrent

目標電流[mA]。
数値を増加させるとサーボの出力軸は時計回りに回転する。

TargetPWM

目標PWM[‰]。
数値を増加させるとサーボの出力軸は時計回りに回転する。
なおPWMモード以外の制御モードにおいて本PWM値が最終段のリミッタとして機能するため、適切な値を設定した上で使用する事。

OffsetTargetAngle

目標角度に加えて指令される値[x100 deg]。
遷移時間を指定して運転している間に偏差を与える場合に使用可。

OffsetTargetVelocity

目標回転数に加えて指令される値[x1000 rot/sec]。
遷移時間を指定して運転している間に偏差を与える場合に使用可。

OffsetTargetVelocity

目標電流に加えて指令される値[mA]。
遷移時間を指定して運転している間に偏差を与える場合に使用可。

CurrentAngle

現在角度[x100 deg]。

CurrentVelocity

現在回転数[x1000 rot/sec]。

CurrentCurrent

現在電流[mA]

CurrentPWM

現在PWM[‰]

CurrentRawPosition

エンコーダの直値。

CurrentBusVoltage

印加された電源の電圧[mV]。

CurrentMPUTemp

MPUから取得された温度[x10 degC]。
仕様上ではキャリブレーションされているとなっているが、どう見ても未キャリブレーション。

CurrentAMPTemp

アンプから取得された温度[x10 degC]。
低インピーダンスなアナログ電圧出力をAD変換しているため、かなり外界に影響されるので参考程度に。

CurrentMotorTemp

モータ近傍に配したPTCから取得した温度[x10 degC]。
標準的な特性曲線から算出しただけなので未キャリブレーション。

AngleViation, VelocityViation, CurrentViation

各制御モードで運転している際の目標値と現在値の偏差。
この値が大きい時は制御能力を超えた状態を意味する。

SysTickTime

ファームウェアが実行開始してからの経過時間[ms]。

ControlDurationTime

Excitationが制御停止以外に設定されてからの経過時間[ms]。

NoCommTime

パケットによる通信が途絶えてからの経過時間[ms]。

AngleCorrection

16bitのエンコーダ値を角度に変換するためのルックアップテーブル。
基準となるエンコーダ値は以下の37箇所で、その時の角度[x100 deg]を設定。
0, 1820, 3641, 5461, 7282, 9102,10923,12743,14563,16384,18204,20025​,21845,23665,25486,27306,29127,30947,327​68,34588,36408,38229,40049,41870,43690,4​5510,47331,49151,50972,52792,54613,56433​,58253,60074,61894,63715,65535
デフォルトは単純な一次関数で算出した角度[x100 deg]を設定。

なお現状ではセンサそのものが未キャリブレーション状態なのと、一定のPWM値で高速回転させるとセンサのキャリブレーションが働いて自動的に校正されてしまうため、本値を編集する事は推奨しない。

MonInfo[0]~MonInfo[4]

異常検出を専用に備えているものではないため、それを補完する目的で装備した監視機能。
5つの監視と指定された条件に一致した時の挙動を設定可能。

ProcModeが発動するとLEDは点滅し、DetectModeの条件から外れるとLEDは消灯する(ProcMode=7を除く)。
ProcModeの0~5が発動している状態でDetectModeの条件が外れると、制限のない制御に復帰する。
監視を複数指定した場合、ProcModeの値が大きいものが優先して実行される。

Control by serial terminal

ここではホストとしてPCを使用する場合を紹介する。
PCにはサーボと通信を行うためのI/Fは装備されていないため、USBポートを介したI/Fを増設して使用する。参考に既成のI/Fをこちらに紹介するが、ここではそのうちDynamixel Starter Kit Bに使用されるI/Oボードを使用した。このI/OボードはサーボとのI/Fの他に、供給用のACアダプタ用が接続できるジャックを備えているため、全ての接続がI/Oボードに集中し構成がシンプルになる。なお搭載しているUSBシリアル変換ICの仕様により、最大通信速度が3Mbpsとなる。

ファームウェアの解説にある通りサーボはテキストベースの通信プロトコルを備えているため、汎用のシリアル通信ターミナルが使用できる。ここではTeraTermを使用して操作する事とした。
TeraTermのシリアルポートの設定は次のように。

またPCから送信するパケットを可視化するために、端末のローカルエコーにチェックを入れておく。

では具体的に通信を行いながら試運転してみる。

手始めにLEDを使った動作の確認を行う。なお送信パケットの末尾に改行などターミネータキャラクタを付与してはならない。

<#1WR3=1> LEDの点灯を指令
(#1WR=OK)
<#1WR3=0> LEDの消灯を指令
(#1WR=OK)

サーボに備わったLEDがパケットに応じて明滅し、応答のパケットが返信されるのを確認できれば最低限の動作環境が整っている事になる。

次にモータを動かす。
内部ではパケットで指定されたコマンドを処理がなされた上で動作するため、必要なコマンドを全て羅列した1つのパケットで処理を完結させる事ができる。
また各制御の最終段にPWM制御(リミッタ)が備わっており、そのリミッタを解除するためにもTPを併記する必要がある。更にカスケード制御時のマイナーコントローラへの指令値は、いずれもその絶対値を使用する。
例えばID=1のサーボを角度を-90deg、回転数を100[r/s]、電流を60mA、PWMリミットなしで制御する場合は以下のように記述する。

<#1EX=7TA=-9000TV=100TC=60TP=1000> 制御指令
(#1EX=OKTA=OKTV=OKTC=OKTP=OK)

これ以降角度のみを変化させるのであれば、角度指令のみを送信しても構わない。

<#1TA=1000>

各制御はPIDアンプを使用しているので、アプリケーションに見合ったゲインに調整した上で運用する事になる。その際に制御指令とゲインの他に現在値取得を追記し、そのパケットを繰り返し送信する事で現在値の変化を確認しながら調整を行う事になる。

<#1TA=5000SG0=2000;5000;0;0CA>
(#TA=OKSG0=OKCA=3540)
<#1TA=5000SG0=2000;5000;0;0CA>
(#TA=OKSG0=OKCA=4302)
<#1TA=5000SG0=2000;5000;0;0CA>
(#TA=OKSG0=OKCA=4933)
<#1TA=5000SG0=2000;5000;0;0CA>
(#TA=OKSG0=OKCA=5010)

しかしこれを手動で行うのは現実的ではないため、後述する数値をグラフで表示するプログラム等を用意するのが良いだろう。

JP200 test app

作者自身がパケットを手動で入力するのが面倒になり、テキストボックスに入力したパケットフレームを送るだけのWindowsアプリを作成。

任意のPortとBaudrateを選択してOpenを押せば準備完了。Transmission dataに送信パケットのテキストを入力しエンタキーを押すと送信され、サーボから応答があればReceived dataにその内容が表示される。またウィンドウを拡大すると、受信パケットの内容をコマンド毎に分解して表示する項目が見えるようになる。
NVMボタンは現在の設定を不揮発メモリに保存、INIボタンは完全初期化、任意のIDを指定して死活確認を行うPing、IDの変更(不揮発化あり)、baudrateの変更(不揮発化無し)する各機能を装備。

サーボのファームウェアはブートローダで紹介した操作によって書き換えができる。しかしTeraTermのマクロを使ってもそれ相応に煩雑な面があるので、本アプリのFirmware Update内にある3つのボタンでも書き換えができる(exeファイルと同一フォルダにJP200.binがある場合のみ使用可)。

2つ目のタブには4種類の波形で各制御値を更新させながら現在値をグラフで確認する機能を設けている。なお2つのタブは各々異なる方法で通信しているため、ポート名とボーレートは1つめタブの設定を共有するが、開閉は排他的になっている。
左上のスライドスイッチでポートの開閉、対象ID・制御モード・遷移時間・波形の周期・波形の種類、各制御の指令値とゲインといった条件や設定が簡易的に試せる。

プロジェクトのReleaseフォルダ内に「jp200_test.exe」として同梱済み。

JP200 mon

teratermやjp200_testを使っても簡単なテキストによるパケットの送受信を確認するのが限界で、様々な値を試しながら調整を行うには役不足である。
そこで通信を行いながら各種の情報をリアルタイムにグラフ表示するアプリケーションを用意した。ボーレートやIDの変更・ID指定による検索・初期化や不揮発化・制御モードの選択・ダイアルや数値による各指令値やゲインの変更・任意波形による指令値の更新・モニタ値のスコープチャート表示・任意パケットの送信といった機能を備えている。

https://www.besttechnology.co.jp/downloa​d/jp200_mon.7z

同梱のjp200_mon.exeはLabVIEWで作られているため、実行するには予めランタイムライブラリをインストールする必要がある。

Control by Python

パケットがテキストベースとは言え、シリアルターミナル上で手動による操作は限界がある。ここでは様々なプラットホームで使用できるPythonを使って通信する方法を紹介する。
ちなみに以下の例はRZboard+DXHATの構成をホストとして使用したものである。

まずパケットの送受信は以下のような感じでいかがだろうか。
ポートとタイムアウト[sec]を指定してpacketクラスを初期化。request関数は指定された条件のパケットを送信し、timeout時間まで「)」が受信されるのを待つ。受信データは「()」で括られた範囲のみを切り出す。複数のサーボを対象にしたパケットの場合はその数だけ受信を繰り返す。requestの引数にpackettype = UNILATERALを指定すると応答無しのパケットを送信、packettype = STRICTを指定するとCRCを付与して送受信。
おまけでモーションを再生するクラスも追加した。

#! python
#
# Communication process when using JP200 from python
#
# SPDX-License-Identifier: MIT
# SPDX-FileCopyrightText: (C) 2023 T.Uemitsu

import enum
import serial, re, threading, time
from collections import namedtuple

#----------------------------------------------------
# Class for sending and receiving packets
#----------------------------------------------------

# Level of packet robustness
class PacketType(enum.Enum):
  CONVENIENCE = 0
  UNILATERAL = 1
  STRICT = 2
globals().update(PacketType.__members__)

class packet:
  p0 = re.compile(r'(#+)=?([;0-9-]+)')
  p1 = re.compile(r'(?<=\().+?(?=\))')
  p2 = re.compile(r'(#|EX|[TC][AVCP]|ST|CT[0-2]|CB|SG[0-2]|INI|VER|RB|WD|WR|:|RR[0-9-]+)=?([;0-9-]+|OK|NG)')
  p3 = re.compile(r':[0-9-]+')
  p4 = re.compile(r':([0-9-]+)')

  __CRC8_SAE_J1850 = [0] * 256

  def __init__(self, ser, timeout = 0.05):
    """Init jp200class.packet object.

    Parameters
    ----------
    - serint (Serial): Initialized Serial object
    - timeout (float): Reception timeout time for one packet
    """
    self.__serial = ser
    self.__serial.timeout = timeout
    self.__mutex = threading.Lock()
    for i in range(256):
      _crc = i
      for b in range(8):
        if _crc & 0x80 != 0: _crc = (_crc << 1) ^ 0x1D
        else:                _crc = (_crc << 1)
      self.__CRC8_SAE_J1850[i] = _crc

  def __crc8(self, st):
    """Calculate CRC of specified string."""
    _crc = 0xff
    for ch in st:
      _crc = self.__CRC8_SAE_J1850[(_crc ^ ord(ch)) & 0xff];
    _crc = _crc ^ 0xff
    return _crc & 0xff

  def request(self, tx, rx = [], packettype = CONVENIENCE, echo = False):
    """Sending and receiving packets."""
    self.__mutex.acquire()
    result = True
    if packettype == CONVENIENCE:
      tx = '<'+tx+'>'
    elif packettype == UNILATERAL:
      tx = '['+tx+']'
    elif packettype == STRICT:
      tx = '{'+tx+':'+str(self.__crc8(tx))+'}'

    if echo: print(tx)
    self.__serial.reset_input_buffer()
    self.__serial.write(tx.encode())
    self.__serial.flush()
    if packettype != UNILATERAL:
      _num = 0
      r = self.p0.findall(tx)
      if r: _num = len(set(r))
      rx[:] = []
      for i in range(_num):
        try:
          r = self.p1.findall(self.__serial.read_until(b')').decode("ascii", "ignore"))
          if r:
            if packettype == STRICT:
              s = self.p3.sub('',r[0])
              if self.__crc8(s) == int(self.p4.findall(r[0])[0]):
                rx.append(r[0])
              else:
                rx.append('')
                result = False
            else:
              rx.append(r[0])
          else:
            result = False
        except:
          rx.append('')
          result = False
      if echo: print(rx)
    self.__mutex.release()
    return result

#----------------------------------------------------
# Class for reproducing discretized trajectories
#----------------------------------------------------

# Minimum Elements of Motion
Pose = namedtuple ('Pose', ('mode', 'targetvalues', 'trajectory', 'time', 'option'))

class motion:
  def __init__(self, pk, hp, maxth = 4):
    """Init jp200class.motion object.

    Parameters
    ----------
    - pk (jp200class.packet): Initialized jp200class.packet object
    - hp (dict): Dict type summarizing ID(int) and initial angle(float)
    - maxth (int): Maximum number of threads
    """
    self.__next_t = [time.time()] * maxth
    self.__motion = [None] * maxth
    self.__index = [0] * maxth
    self.__thread = [None] * maxth
    self.__pk = pk
    self.__hp = hp
    self.__IDs = [()] * maxth
    self.__maxth = maxth

  def __del__(self):
    for i in range(self.__maxth):
      self.stop(i)

  def __run(self, *arg):
    """Threads called for the first time and at every transition time"""
    no = arg[0]
    if self.__maxth > no:
      if len(self.__motion[no]) >= self.__index[no] + 1:
        m = self.__motion[no][self.__index[no]]
        acks = []
        s = ''
        n = 0
        for id, tval, h in zip(self.__IDs[no], m.targetvalues, self.__hp):
          if m.mode == 1 or m.mode == 3 or m.mode == 5 or m.mode == 7:
            s += ('#%dEX=%dTA=%d;%d;%d%s'%(id, m.mode, (tval + self.__hp[id]) * 100, m.trajectory, m.time, m.option))
          elif m.mode == 2 or m.mode == 6:
            s += ('#%dEX=%dTV=%d;%d;%d%s'%(id, m.mode, tval, m.trajectory, m.time, m.option))
          elif m.mode == 4:
            s += ('#%dEX=%dTC=%d;%d;%d%s'%(id, m.mode, tval, m.trajectory, m.time, m.option))
          elif m.mode == 8:
            s += ('#%dEX=%dTP=%d%s'%(id, m.mode, tval, m.option))
          else:
            s += ('#%dEX=%d%s'%(id, m.mode, m.option))
          n += 1
        self.__pk.request(s, acks, UNILATERAL)
        self.__next_t[no] += m.time / 1000.0
        self.__index[no] += 1
        if (self.__index[no] <= len(self.__motion[no])):
          self.__thread[no] = threading.Timer(self.__next_t[no] - time.time(), self.__run, [no])
          self.__thread[no].start()
      else:
        self.__stop(no);

  def __stop(self, no):
    """Local end-of-timer processing."""
    if self.__maxth > no:
      if self.__thread[no] != None:
        self.__thread[no].cancel()
        self.__thread[no] = None
      return True

  def stop(self, no):
    """Stop process including 'EX=14'."""
    if self.__stop(no):
      s = ''
      for id in self.__IDs[no]:
        s += ('#%dEX=14'%(id))
      self.__pk.request(s, packettype = UNILATERAL)

  def start(self, no, ids, motion):
    """Start of motion."""
    if self.__stop(no):
      if len(motion) > 0:
        self.__IDs[no] = ids
        self.__motion[no] = motion
        self.__index[no] = 0
        self.__next_t[no] = time.time()
        self.__run(no)

  def running(self, no):
    """Detects motion in progress."""
    if self.__maxth > no:
      return self.__thread[no] != None
    else:
      return False

クラスを使って実際にパケットを送ってみる。request関数には括弧を除くパケットの中身を記述、受信データは文字列のlistで受け取る。ここではひとまずデバッグ目的でecho=Trueを指定。

import serial, time
from jp200class import packet
#シリアルポートを開く
ser = serial.Serial('/dev/ttySC2', 1000000)
pk = packet(ser, 0.02)
#応答の受信用変数
acks = []
#角度制御のひな形
ang = '#%dEX=1TA=%dTP=1000'
#ID=1へ180度の角度を指令
pk.request(ang%(1, 18000), acks, echo=True)
time.sleep(0.5)
#ID=1へ90度の角度を指令
pk.request(ang%(1, 9000), acks, echo=True)
time.sleep(0.5)
#ID=1へ270度の角度を指令
pk.request(ang%(1, 27000), acks, echo=True)
ser.close()

実行するとターミナル上に以下の表示がなされる。

<#1EX=1TA=18000TP=1000>
['#1EX=OKTA=OKTP=OK']
<#1EX=1TA=9000TP=1000>
['#1EX=OKTA=OKTP=OK']
<#1EX=1TA=27000TP=1000>
['#1EX=OKTA=OKTP=OK']
>>>

次に受信したパケットを正規表現任せでコマンド毎に分解してみる。正規表現パターンはクラスに定義済みなのでそれを利用。

import serial
from jp200class import packet
ser = serial.Serial('/dev/ttySC2', 1000000)
pk = packet(ser, 0.02)
acks = []
#ID=1へ180度の角度指令と合わせて角度・角速度・電流・電源電圧を取得
pk.request('#1EX=1TA=18000TP=500CACVCCCB', acks)
#応答が得られた時のみ正規表現でパケットを分離して表示
if acks:
  for ack in acks:
    m = pk.p2.findall(ack)
    for m in m:
      print(m)
ser.close()

コマンドと結果がタプルとして表示される。

('#', '1')
('EX', 'OK')
('TA', 'OK')
('TP', 'OK')
('CA', '17792')
('CV', '0')
('CC', '0')
('CB', '12168')
>>>

分離しただけでは意味が無いので、数値変換してID毎にリストに保存してみる。RRコマンドは';'をデリミタとしてデータが連なって返ってくるのでsplitを使って更に分割。

import serial
import jp200class as jp200

ser = serial.Serial('/dev/ttySC2', 1000000)
pk = jp200.packet(ser, 0.02)

acks = []
#軸数分の情報を保存するリスト
CurrentValues = [[0] * 15] * 2;

#ID=1とID=2のコントロールテーブルの45から15個のデータを読み出し
if pk.request('#1RR45;15#2RR45;15', acks):
  # 軸数分ループ
  for ack in acks:
    # コマンド毎に分離
    r = pk.p2.findall(ack)
    if r:
      ind = -1
      # コマンド数分ループ
      for _r in r:
        if _r[0] == '#' and int(_r[1]) <= 2: ind = int(_r[1]) - 1
        if _r[0] == 'RR45' and ind >= 0: CurrentValues[ind] = [int(s) for s in _r[1].split(';')]
  # 取得した現在値の一部を表示
  print ('[0] {0[0]:6} {0[1]:5} {0[2]:5}{0[12]:10} [1] {1[0]:6} {1[1]:5} {1[2]:5} {1[12]:10}'.format(*CurrentValues))
ser.close()

[0]  27041     0   -19   1211739 [1]  17892     0     0    1211737
>>>

モーションは時間を追って変化する目標値を任意の時間で区切って羅列したデータを予め作っておき、そのデータをサーボの補間機能を活用しながら再生するもの。ここでは2個のサーボを角度制御と速度制御によるモーションを使って運転してみる。

import serial
import jp200class as jp200

portname = '/dev/ttySC2'
#portname = '\\\\.\\COM12'
baudrate = 1000000
HomeOffsetAngle = {1:180.0, 2:180.0}

# Target ID
IDs = (1,2)

# Motion data
m0 = (
  jp200.Pose(1, [   0.0,   0.0 ], 1, 1000, 'TP=1000'),
  jp200.Pose(1, [  90.0,  10.0 ], 1, 1500, ''),
  jp200.Pose(1, [ 180.0,  20.0 ], 1, 1500, ''),
  jp200.Pose(1, [ -90.0, -30.0 ], 1, 1500, ''),
)

m1 = (
  jp200.Pose(1, [   0.0,   0.0 ], 1, 1000, 'TP=1000'),
  jp200.Pose(1, [ 200.0, 200.0 ], 1,  500, ''),
  jp200.Pose(1, [ 100.0, 100.0 ], 1,  500, ''),
  jp200.Pose(1, [-100.0,-100.0 ], 1,  500, ''),
  jp200.Pose(1, [   0.0,   0.0 ], 1,  500, ''),
)

ser = serial.Serial(portname, baudrate)
pk = jp200.packet(ser, 0.02)
mt = jp200.motion(pk, HomeOffsetAngle)

s = ''
while s != 'q':
  s = input('>')
  if s == '1':
    mt.start(0, IDs, m0)
  if s == '2':
    mt.start(0, IDs, m1)
  if s == '3':
    mt.start(0, IDs, (jp200.Pose(2, [ 400.0, 800.0 ], 1, 500, 'TP=1000'),))
  if s == '4':
    mt.start(0, IDs, (jp200.Pose(2, [ -800.0, -400.0 ], 1, 500, 'TP=1000'),))
  if s == '5':
    mt.start(0, IDs, (jp200.Pose(2, [ 0.0, 0.0 ], 1, 500, 'TP=1000'),))
  if s == 'e':
    print('stop')
    for i in range(4):mt.stop(i)

for i in range(4):mt.stop(i)
del mt, pk
ser.close()

最後にもう一つ。サーボのIDやボーレートを変更する場合はjp200_testで行えるが、Pythonでも変更するコードを紹介しておく。

#!/usr/bin/python3
import time, sys, serial, re, os
from jp200class import packet

hostbaud = 1000000
baudlist = [9600, 19200, 38400, 57600, 100000, 115200, 200000, 230400, 460800, 500000, 1000000, 2000000, 3000000, 4000000]
term = False

def hlp(v):
  print(' id CurrentID ChangedID        : Change targett ID')
  print(' baud [baudrate | ID baudrate] : Change host or target baudrate')
  print(' ping ID                       : Check response for current ID')
  print(' scan                          : Scan all id ranges')
  print(' scan2                         : Scan all baudrate and id ranges')
  print(' nvm ID                        : Make settings non-volatile')
  print(' quit                          : Quit')
  return True

def bye(v):
  print("Bye!");
  global term
  term = True
  return True

def _ping(id):
  ack = []
  pk.request('#%d'%(id), ack)
  if ack:
    return '#%d'%(id) in ack[0]
  return False

def Ping(v):
  print(" Ping")
  if len(v) == 2:
    if v[1].isdecimal():
      if _ping(int(v[1])): print (" Found %d"%(int(v[1])))
      else: print (" Not found %d"%(int(v[1])))
  else:
    print(" missing argument !!")
  return False

def Scan(v):
  detect = False
  print(" Scan")
  if len(v) == 1:
    try:
      for id in range(0, 254):
        if _ping(id):
          detect = True
          print('(%d)'%(id), end='', flush = True)
        else:
          print('.', end='', flush = True)
      print()
    except KeyboardInterrupt:
      print();
  else:
    print(" missing argument !!")
  return detect

def Scan2(v):
  detect = False
  print(" Scan2")
  if len(v) == 1:
    try:
      for b in baudlist:
        print ('baudrate=',b);
        ser.baudrate = b
        for id in range(0, 254):
          if _ping(id):
            detect = True
            print('(%d)'%(id), end='', flush = True)
          else:
            print('.', end='', flush = True)
        print()
    except KeyboardInterrupt:
      print();
  else:
    print(" missing argument !!")
  return detect

def ChangeTargetID(v):
  global hostid
  print(" Change target ID")
  if len(v) == 3:
    if v[1].isdecimal():
      curid = int(v[1])
      if v[2].isdecimal():
        changedid = int(v[2])
        if changedid <= 254:
          if _ping(curid) and not _ping(changedid):
            ack = []
            pk.request( '#%dWR0=%d'%(curid, changedid), ack)
            if ack:
              if '#%dWR=OK'%(curid) in ack[0] and _ping(changedid):
                #hostid = changedid
                print(" Changed ID %d to %d."%(curid, changedid))
                return True
    print(" Fail to change")
  else:
    print(" missing argument !!")
  return False

def ChangeTargetBaud(v):
  global hostbaud
  if len(v) == 2:
    print(" Change host baudrate");
    if v[1].isdecimal():
      if int(v[1]) in baudlist:
        ser.baudrate = int(v[1])
        hostbaud = int(v[1])
        return True
  elif len(v) == 3:
    print(" Change target baudrate");
    if v[1].isdecimal():
      id = int(v[1])
      if v[2].isdecimal():
        changedbaud = int(v[2])
        if changedbaud in baudlist:
          baudind = baudlist.index(changedbaud)
          ser.baudrate = changedbaud
          if not _ping(id):
            ser.baudrate = hostbaud
            ack = []
            pk.request( '#%dWR1=%d'%(id, baudind), ack)
            if ack:
              if '#%dWR='%(id) in ack[0]:
                ser.baudrate = changedbaud
                if _ping(id):
                  hostbaud = changedbaud
                  return True
  else:
    print(" missing argument !!")
  return False

def WriteNVM(v):
  global hostid
  print(" WriteNVM")
  if len(v) == 2:
    if v[1].isdecimal():
      id = int(v[1])
      ack = []
      pk.request( '#%dWD'%(id), ack)
      if ack:
        print(ack)
        return '#%dWD=OK'%(id) in ack[0]
  else:
    print(" missing argument !!")
  return False

cmdlist = {"help":hlp, "?":hlp, "quit":bye, "q":bye, "ping":Ping, "scan":Scan, "scan2":Scan2, "id": ChangeTargetID, "baud": ChangeTargetBaud, "nvm": WriteNVM}

try:
  ser = serial.Serial('\\\\.\\COM12', hostbaud)
  #ser = serial.Serial('/dev/ttySC2', hostbaud, timeout = 0.02)
  print (ser.name)
  pk = packet(ser, 0.02)
  k = ''
  cmd = []
  while not term:
    k = input('%dbps>'%(hostbaud))
    cmd = k.split(' ')
    if cmd[0] in cmdlist:
      result = cmdlist[cmd[0]](cmd)
  ser.close()
except serial.serialutil.SerialException:
  pass
os._exit(0)

Issues

• 測定値を元に故障や危険を察知して停止させる事は一切行っていない。
  諸条件を監視機能へ設定する事で出力制限をかける事が可能。
• 電源電圧の変動が大きいと非接触エンコーダが低電圧を検出する事があるが、その場合取得される角度情報の更新が停止する。その状態で制御すると暴走につながるので注意。
  異常検出時はLEDを点滅させ制御を強制停止。
• 電源を逆接続すると永久破壊。
• 非接触エンコーダは出荷時状態のため、ゼロ位置不定・位置の偏心・速度制御の揺動・運転中の異常検出時にカウント停止が発生する。出力軸のマーカーは何の意味も持たない。
• エンコーダから得られた値をルックアップテーブルを用いて角度にしている。動作範囲が制限される機構(関節等)に適用する場合は、180度(18000)を示す位置と可動範囲の中央が一致するようにする事。通電直後の0度近傍はオーバーフローの危険がある。
• 0~360度を超えた角度を指令したり取得する事ができるが、パワーサイクルによって0~360度の範囲にクリップされる。
• 半二重通信のドライバとしてGreenPAKを使用しているが、電源の入り切りのタイミングで無用な通信データが排出される。
• 通信バスのプルアップがウィークなため、状況によってはブートローダの通信に支障が生じる事がある。ファームウェアの更新の際はプルアップを強化する事を強く推奨する。
• 元来電流センサがPWMに対応していないため、電流制御を行う際はリプルが顕著にならないレベルで行う事。
• 不揮発データの保存は常時失敗を返すが、それはfspのAPIを使ったデータフラッシュへの書き込み時の戻り値をそのまま返しているからだが、実際には書き込まれている。
• 割り込み処理内でのI2Cアクセス処理が過大。
• 通信バッファはMPUの大きめなリソース頼みで制限を設けていない。
• 各制御値の上下限リミッタが甘い。特に長い移動時間と大きい目標値を指定すると簡単に破綻する。
• 温度センサ周辺は理論値によるものでキャリブレーションは行われていない。
• プロジェクトの最適化レベルを上げると想定したロジックが破綻するため、苦肉の策としてソースレベルで最適化レベルを変更するようにしている。
• 製造済みの実機には古いブートローダが書き込まれているため、ユーザプログラムのXMODEMによる吸い出しは正常に機能しない。
  改修したブートローダで修正済み。
• VBUSの検出端子が他の目的で使用されているため、USBによる運用は推奨されない。
• ここだけの話、TrustZoneの罠とデバッガのバグ等に嵌まってレンガにしたチップは数知れず。いざという時のためにSWD以外にSCIブートモードで使用される端子を引き出せるようにしておくべきのようだ。