|
一覧
検索
最新
ページへ戻る
履歴
JP200(PoC)
のソース
knowledge
:
JP200(PoC)
のソース
差分
を表示
JP200(PoC)
へ行く。
#norelated #contents [[ロボットの手足用サーボモータ>https://www.renesas.com/jp/ja/application/key-technology/motor-control-robotics/servo-motor-control-robot-limbs]] **Overview [#w9c7482b] ルネサスエレクトロニクス社(以後renesas)のコンポーネントを使用したブラシ付きDCモータを簡易的に制御するPoC(概念実証)ベンチ。~ 外観は既成のサーボモータと酷似し、簡単なテキストコマンドにより角度・速度・電流の各制御指令を行う。 **Specification[#x4c038d0] ***Fundamental spec. [#p4a04946] |BGCOLOR(#d0d8e0):CENTER:|LEFT:|LEFT:|c |型番|JP200 |<| |PCBリビジョン|E192(4層) |<| |動作温度範囲|-10~50℃ 結露なきこと|<| |外形寸法|41×21×30.5mm(突起部除く) |<| |重量|41.5g(アクセサリを除く) |<| |電源|DC6~12V(絶対最大定格DC28V) |<| |モータ|コアドブラシ付DCモータ|<| |ストールトルク|1.37N•m(at 11.1V, 1.3A)|<| |無負荷回転数|50rpm(at 11.1V)|<| |減速比|1/312.4|<| |最大動作角度|位置決め制御時:0~360°(16ビット分解能),Endless Turn|<| |コネクタ等|JST B3B-ZR x2&br;LED x1 |<| |I/F仕様|半二重TTLレベル |<| 機構はKONDO [[KRS-2500シリーズ>https://kondo-robot.com/faq/krs-2552hv-diagram]]と互換性があるので、それ用に提供されるパーツが流用可能。 ***Layout [#m234f98d] ||| ||| #ref(E192_LAYOUT.pdf) ***Schematic [#ma437e61] #ref(E192_SCH.pdf) ***Terminals [#pbd48807] ****CN1, CN2 [#p389d37c] #ref(JP200_CN1_CN2_LED.png) -電源並びに通信用信号ラインを接続 |Pats Name |JST Parts Number | |基板用ヘッダー |[[B3B-ZR>https://www.jst-mfg.com/product/index.php?series=287]] | |ハウジング |[[ZHR-3>https://www.jst-mfg.com/product/index.php?series=287]] | |ターミナル |[[SZH-002T-P0.5>https://www.jst-mfg.com/product/index.php?series=287]] | #ref(E192_CN1_CN2_POW,45%) | 端子番号 | 信号名 | |h |CENTER:60|CENTER:|LEFT:|c |1|Signal|シングルエンドのシリアル通信信号&br;双方向 | |2|VDD|電源のプラス側 | |3|GND|電源のマイナス側&br;シリアル通信信号の基準電位と共有 | ****LED [#wf58d9df] -緑色LED -アプリケーションにてプログラマブルに点灯・消灯 -電流制限抵抗を介してMPUのGPIOにソース接続 ****CN3 [#l28a2141] #ref(JP200_CN3.png) -パッドのみ -USBにかかる信号を接続 #ref(E192_CN3.png) ****CN4 [#f6f9b62b] #ref(JP200_CN4.png) -ランドのみ -SWDにかかる信号を接続 #ref(E192_CN4.png) **Details of each function [#f83865b0] ***Bootloader [#s6b7645f] > 初期ブートローダ~ https://www.besttechnology.co.jp/download/JP200_bootloader_20230429.7z -fsp4.4 > 2023/9/23更新~ https://www.besttechnology.co.jp/download/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でプロジェクトを作成する際の要点は[[こちら>RA4M2#s37a7b5f]]を参考に。 ****Boot conditions [#meb0621a] コールドスタートやホットスタート、ユーザープログラムの状態による分岐を以下に記載する。 -実行可能なユーザプログラムが書き込み済み ~電源を印加するとブートローダはそのユーザプログラムにジャンプ。~ I/Fから'!'を連続で受信させながら電源を印加するとブートローダはコマンドモードで待機。 -ユーザプログラムが消去もしくは実行不可 ~電源を印加するとブートローダはコマンドモードで待機。 -ユーザプログラム内でソフトウェアリセット発行 ~ソフトウェアリセット後にブートローダはコマンドモードで待機。 ****Transfer and execution of user programs [#e3f7ac74] ユーザプログラムの転送にはレガシーなXMODEM/1kプロトコルのみに対応1し、I/Fの設定は1Mbps 8N1を要求する。XMODEMが半二重を前提としているので、ホスト側のターミナルソフトウェアはI/Fが半二重であることを特段意識する事はない。 ユーザープログラムの転送手順は以下の通り。~ まず'e'を送信してフラッシュメモリの消去を起動する。 #html{{ <pre class="brush: bash;"> RA4M2 BL1.2>e(y/n) </pre> }} ここで'y'を送信すると実際に消去される。 #html{{ <pre class="brush: bash;"> RA4M2 BL1.2>e(y/n)y OK RA4M2 BL1.2> </pre> }} 次に'w'を送信して転送モードを起動する。 #html{{ <pre class="brush: bash;"> RA4M2 BL1.2>w(y/n) </pre> }} ここでyを入力したら、すかさず転送プロトコルに従って任意のバイナリファイルの送信を開始させる。成功すれば<OK>、失敗すると<NG>と表示される。 #html{{ <pre class="brush: bash;"> RA4M2 BL1.2>w(y/n)y OK RA4M2 BL1.2> </pre> }} 'g'を送信してフラッシュメモリに書き込まれたユーザプログラムを実行する。 #html{{ <pre class="brush: bash;"> RA4M2 BL1.2>g </pre> }} これらの一連の操作を[[TeraTerm>http://ttssh2.osdn.jp/]]で行う際のマクロを紹介しておく。 #ref(upload_bin.ttl) ***Firmware [#mbb224a3] > 初期ファームウェア~ https://www.besttechnology.co.jp/download/JP200_20230721.7z -fsp4.5 > 2023/9/18更新~ https://www.besttechnology.co.jp/download/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/download/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/download/JP200_20231011.7z -fsp4.6 -各制御にフィードフォワードを追加~ それに伴いSG0~2コマンドの引数を追加 -各制御ゲインの上限値を30000に変更 -各制御における指令値に加算するオフセット指令値を追加 -日替わりで更新している事もあり、ひとまずVERコマンドを追加 -TA/TV/TC/SGコマンドの数値切り出しに失敗するようなので処理方法を変更 -RRコマンドで温度の更新がなされないのを修正 -通信が復帰しないケースが散見されたので、暫定的にuartのコールバック中にエラー処理を追加 -コントロールテーブルのサイズ及びアイテムのインデックスが更新されたため、本ファームウェアを適用すると不揮発領域は初期化される > 2023/10/30更新~ https://www.besttechnology.co.jp/download/JP200_20231030.7z -fsp5.0 -すっかり失念していたCRCの付与に対応 -非接触エンコーダとの通信に使っているCRCルーチンを分離 -少し攻め気味だった速度制御のゲインを低めに変更 -制御モードに静止を追加 -大きめのTransitionTimeと目標値を指定すると容易にオーバーフローするので計算は64bitに変更 -角度制御以外から角度制御に切り替えた直後に遷移時間を指定して運転すると開始角度が初期化されないのを修正 -同梱のjp200_test.exeを更新 ****General features [#la2508c5] 一般的な算数通りに演算してモータをフィードバック制御しているのみで、特筆すべき所は無い。唯一あるとすればシリアルサーボの類にありがちなバイナリベースの通信プロトコルを一切使わず、全て目視で判断できるテキストのみの電文で処理している点。なおI/Fは1ワイヤの半二重通信を前提とし、昨今のデバイスの能力に任せてメガbpsでの通信を基本とする。~ ファームウェアはe2studioを用いて作られており、renesasから提供されるfspを一通り使用した。開発当初はSWD端子にSEGGER社のJ-Linkを接続してデバッグを行っていたが、アプリケーションがテキストによる通信を行うもののため、最終的にはブートローダを介したファームウェアの書き換えとprintfデバッグを併用した。 なおソース単位で機能が囲われているかのように見えるかも知れないが、シングルソースだったものを後にそれっぽく分割した程度である。またC++でクラス化していた時分もあったが、積極的に使い始めた途端に破綻したのでCに戻した経緯がある。 ****Communication protocol [#k5bcddaf] ホストもしくはサーボのパケットである事を区別するため、それぞれ不等号の「''<>''」と括弧の「''()''」のヘッダとフッダで囲われた範囲を有効なパケットとする。このヘッダとフッダに挟まれた中に必要な命令や応答を記述するが、使用できるのはアスキー文字の「''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 [#ib869773] 以下の「コマンド」に記述された「''<''」「''>''」「''[''」「'']''」はコマンドの構文の説明としての記号であり、「''<>''」内は必須、「''[]''」内はオプションを説明しているもので、実際にパケット中にそれらの記号を記述してはならない。また数値は全て10進数表記である。 |CENTER:|CENTER:|LEFT:|c |機能|コマンド| 指令 |h |~|~| 応答 |h |ID指定|''#''<n>|n=0~254&br;コマンドの送信先を指定するために必ず記述必要があり、複数のIDを混在させる事も可| |~|~|n=0~254&br;同一IDを複数指定した場合、1つにまとめられる| |制御モード変更|''EX=''<n>|n=0~8,15&br;0:制御停止,&br;1:角度制御,&br;2:角速度制御,&br;3:角度+速度制御,&br;4:電流制御,&br;5:角度+電流制御,&br;6:速度+電流制御,&br;7:角度+速度+電流制御,&br;8:PWM,&br;14:現在位置を維持したまま静止,&br;15:回生ブレーキ| |~|~|n=OK または NG| |角度指令|''TA=''<n[['';''m['';''t]]]>|n=指令値[x100 deg]&br;m=遷移軌跡&br;t=遷移時間[ms]| |~|~|n=OK または NG| |角速度指令|''TV=''<n[['';''m['';''t]]]>|n=指令値[x1000 rot/sec]&br;m=遷移軌跡&br;t=遷移時間[ms]| |~|~|n=OK または NG| |電流指令|''TC=''<n[['';''m['';''t]]]>|n=指令値[mA]&br;m=遷移軌跡&br;t=遷移時間[ms]| |~|~|n=OK または NG| |PWM指令|''TP=''<n>|n=指令値[‰]| |~|~|n=OK または NG| |現在角度取得|''CA''|現在角度を取得| |~|''CA=''<n>|n=フィードバック値[x100 deg]| |現在角速度取得|''CV''|現在角速度を取得| |~|''CV=''<n>|n=フィードバック値[x1000 rot/sec]| |現在電流取得|''CC''|現在角電流を取得| |~|''CC=''<n>|n=フィードバック値[mA]| |現在PWM取得|''CP''|現在PWMを取得| |~|''CP=''<n>|n=フィードバック値[‰]| |MPU温度取得|''CT0''|MPU温度を取得| |~|''CT0=''<n>|n=フィードバック値[x10 degC]| |アンプ温度取得|''CT1''|アンプ温度を取得| |~|''CT1=''<n>|n=フィードバック値[x10 degC]| |モータ温度取得|''CT2''|モータ温度を取得| |~|''CT2=''<n>|n=フィードバック値[x10 degC]| |電源電圧取得|''CB''|電源電圧を取得| |~|''CB=''<n>|n=フィードバック値[mV]| |ステータス取得|''ST''|内部ステータスを取得| |~|''ST=''<n>|n=ステータス| |位置制御ゲイン設定|''SG0=''<p['';''i['';''d['';''f]]]>|p=比例ゲイン, i=積分ゲイン, d=微分ゲイン, f=フィードフォワードゲイン&br;ゲインはいずれも整数で、1/1000の固定小数点として扱われる | |~|''SG0=''<n>|n=OK または NG| |角速度制御ゲイン設定|''SG1=''<p['';''i['';''d['';''f]]]>|p=比例ゲイン, i=積分ゲイン, d=微分ゲイン, f=フィードフォワードゲイン&br;ゲインはいずれも整数で、1/1000の固定小数点として扱われる | |~|''SG1=''<n>|n=OK または NG| |電流制御ゲイン設定|''SG2=''<p['';''i['';''d['';''f]]]>|p=比例ゲイン, i=積分ゲイン, d=微分ゲイン, f=フィードフォワードゲイン&br;ゲインはいずれも整数で、1/1000の固定小数点として扱われる | |~|''SG2=''<n>|n=OK または NG| |テーブル書込|''WR''<ind>''=''<d1>['';''<d2>]['';''<d3>]...['';''<dN>]|ind=開始インデックス&br;d1~dN=データ列| |~|''WR''<ind>''=''<n>|ind=開始インデックス&br;n=OK または NG| |テーブル読出|''RR''<ind>'';''<n>|ind=開始インデックス&br;n=データ個数| |~|''RR''<ind>''=''<d1>['';''<d2>]['';''<d3>]...['';''<dN>]|ind=開始インデックス&br;n=データ列| |不揮発化|''WD''|NVMとあるアイテムの情報をデータフラッシュに保存する | |~|''WD=''<n>|n=OK または NG| |初期化|''INI''|全ての情報をデフォルトに戻しデータフラッシュに保存する| |~|''INI=''<n>|n=OK または NG| |再起動|''RB''|ソフトリセットを行いブートローダをコマンドモードで立ち上げる | |~|''RB=''<n>|n=OK または NG| |構築日取得|''VER''|プロジェクトの再構築日時を取得| |~|''VER=''<Y'';''M'';''D'';''h'';''m'';''s>|Y/M/D:日付、h/m/s:時刻| ****Control table [#c3634246] TAやTVといったコマンドは、実際には次に示すコントロールテーブルへのアクセスに変換される。コントロールテーブルへの直接的なアクセスにはWRやRRコマンドを使用する。 |=''Index''|=''Item''|<|=''Access''|=''Default Value''|=''Type/Range''|h |CENTER:|LEFT:|<|CENTER:|CENTER:|CENTER:|c |0|BGCOLOR(seashell):[[ID>#ic849fe1]]|<|R/W (NVM)|1|0 ~ 254| |1|BGCOLOR(seashell):[[Baudrate>#l5c8d576]]|<|R/W (NVM)|10|0 ~ 14| |2|BGCOLOR(seashell):[[OffsetAngle>#x7adf8c1]]|<|R/W (NVM)|0|-18000 ~ 18000| |3|BGCOLOR(lightcyan):[[LED>#q88f36d9]]|<|R/W|0|0 ~ 1| |2|BGCOLOR(seashell):[[ReturnDelayTime>#d2f1f42d]]|<|R/W (NVM)|0|0 ~ 1000| |5|BGCOLOR(beige):[[SystemStatus>#xea777f0]]|<|R|-|-| |6|BGCOLOR(seashell):[[AngleFlexibility>#a735883f]]|BGCOLOR(seashell):[[n_compliance>#a735883f]]|R/W (NVM)|0|-36000 ~ 0| |7|~|BGCOLOR(seashell):[[n_deadzone>#a735883f]]|R/W (NVM)|0|-36000 ~ 0| |8|~|BGCOLOR(seashell):[[p_deadzone>#a735883f]]|R/W (NVM)|0|0 ~ 36000| |9|~|BGCOLOR(seashell):[[p_compliance>#a735883f]]|R/W (NVM)|0|0 ~ 36000| |10|BGCOLOR(seashell):[[CurrentFlexibility>#wd5120f1]]|BGCOLOR(seashell):[[n_compliance>#wd5120f1]]|R/W (NVM)|0|-1300 ~ 0| |11|~|BGCOLOR(seashell):[[n_deadzone>#wd5120f1]]|R/W (NVM)|0|-1300 ~ 0| |12|~|BGCOLOR(seashell):[[p_deadzone>#wd5120f1]]|R/W (NVM)|0|0 ~ 1300| |13|~|BGCOLOR(seashell):[[p_compliance>#wd5120f1]]|R/W (NVM)|0|0 ~ 1300| |14&br;~&br;17|BGCOLOR(silver):(reserve)|<|R|-| | |18|BGCOLOR(seashell):[[AngleLimit>#f665309d]]|BGCOLOR(seashell):[[min>#f665309d]]|R/W (NVM)|-2147483648|-2147483648 ~ 2147483647| |19|~|BGCOLOR(seashell):[[max>#f665309d]]|R/W (NVM)|2147483647|-2147483648 ~ 2147483647| |20|BGCOLOR(seashell):[[AngleCtrlGain>#g5d932a9]]|BGCOLOR(seashell):[[Kp>#g5d932a9]]|R/W (NVM)|2000|0 ~ 30000| |21|~|BGCOLOR(seashell):[[Ki>#g5d932a9]]|R/W (NVM)|5000|0 ~ 30000| |22|~|BGCOLOR(seashell):[[Kd>#g5d932a9]]|R/W (NVM)|0|0 ~ 30000| |23|~|BGCOLOR(seashell):[[Kf>#g5d932a9]]|R/W (NVM)|0|0 ~ 30000| |24|~|BGCOLOR(seashell):[[delta_t>#g5d932a9]]|R/W (NVM)|1|1 ~ 1000| |25|BGCOLOR(seashell):[[VelocityCtrlGain>#g5d932a9]]|BGCOLOR(seashell):[[Kp>#g5d932a9]]|R/W (NVM)|400|0 ~ 30000| |26|~|BGCOLOR(seashell):[[Ki>#g5d932a9]]|R/W (NVM)|600|0 ~ 30000| |27|~|BGCOLOR(seashell):[[Kd>#g5d932a9]]|R/W (NVM)|0|0 ~ 30000| |28|~|BGCOLOR(seashell):[[Kf>#g5d932a9]]|R/W (NVM)|0|0 ~ 30000| |29|~|BGCOLOR(seashell):[[delta_t>#g5d932a9]]|R/W (NVM)|32|1 ~ 1000| |30|BGCOLOR(seashell):[[CurrentCtrlGain>#g5d932a9]]|BGCOLOR(seashell):[[Kp>#g5d932a9]]|R/W (NVM)|900|0 ~ 30000| |31|~|BGCOLOR(seashell):[[Ki>#g5d932a9]]|R/W (NVM)|1500|0 ~ 30000| |32|~|BGCOLOR(seashell):[[Kd>#g5d932a9]]|R/W (NVM)|0|0 ~ 30000| |33|~|BGCOLOR(seashell):[[Kf>#g5d932a9]]|R/W (NVM)|0|0 ~ 30000| |34|~|BGCOLOR(seashell):[[delta_t>#g5d932a9]]|R/W (NVM)|2|1 ~ 1000| |35|BGCOLOR(lightcyan):[[TransitionMode>#v5e900e3]]|<|R/W|0|0 ~ 3| |36|BGCOLOR(lightcyan):[[TransitionTime>#p91011e1]]|<|R/W|0|0 ~ 300000| |37|BGCOLOR(lightcyan):[[Excitation>#cff8d1dd]]|<|R/W|0|0 ~ 15| |38|BGCOLOR(lightcyan):[[TargetAngle>#qce18229]]|<|R/W|-|-2147483648 ~ 2147483647| |39|BGCOLOR(lightcyan):[[TargetVelocity>#mb5bcdb9]]|<|R/W|0|-2000 ~ 2000| |40|BGCOLOR(lightcyan):[[TargetCurrent>#i34397b2]]|<|R/W|0|-1300 ~ 1300| |41|BGCOLOR(lightcyan):[[TargetPWM>#sb6d131e]]|<|R/W|0|-1000 ~ 1000| |42|BGCOLOR(lightcyan):[[OffsetTargetAngle>#w7715279]]|<|R/W|0|-2147483648 ~ 2147483647| |43|BGCOLOR(lightcyan):[[OffsetTargetVelocity>#d3138ca2]]|<|R/W|0|-2000 ~ 2000| |44|BGCOLOR(lightcyan):[[OffsetTargetCurrent>#u50c65d4]]|<|R/W|0|-1300 ~ 1300| |45|BGCOLOR(beige):[[CurrentAngle>#eb5380bc]]|<|R|-|-| |46|BGCOLOR(beige):[[CurrentVelocity>#n780b169]]|<|R|-|-| |47|BGCOLOR(beige):[[CurrentCurrent>#lf283fd1]]|<|R|-|-| |48|BGCOLOR(beige):[[CurrentPWM>#g43a4d35]]|<|R|-|-| |49|BGCOLOR(beige):[[CurrentRawPosition>#c6249941]]|<|R|-|0~65535| |50|BGCOLOR(beige):[[CurrentBusVoltage>#v51442a1]]|<|R|-|-| |51|BGCOLOR(beige):[[CurrentMPUTemp>#v4b8fb0d]]|<|R|-|-| |52|BGCOLOR(beige):[[CurrentAMPTemp>#taa9d975]]|<|R|-|-| |53|BGCOLOR(beige):[[CurrentMotorTemp>#ab1de60b]]|<|R|-|-| |54|BGCOLOR(beige):[[AngleViation>#jd896584]]|<|R|-|-| |55|BGCOLOR(beige):[[VelocityViation>#jd896584]]|<|R|-|-| |56|BGCOLOR(beige):[[CurrentViation>#jd896584]]|<|R|-|-| |57|BGCOLOR(beige):[[SysTickTime>#fb9c6462]]|<|R|-|-| |58|BGCOLOR(beige):[[ControlDurationTime>#c7d76d53]]|<|R|-|-| |59|BGCOLOR(beige):[[NoCommTime>#p75778fa]]|<|R|-|-| |60&br;~&br;96|BGCOLOR(seashell):[[AngleCorrection[0~36]>#y925e42a]]|<|R/W (NVM)| |0 ~ 36000| |97|BGCOLOR(seashell):[[MonInfo[0]>#g03b20b0]]|BGCOLOR(seashell):[[ItemIndex>#g03b20b0]]|R/W (NVM)|-1|-1 ~ 131| |98|~|BGCOLOR(seashell):[[min>#g03b20b0]]|R/W (NVM)|0|-2147483648 ~ 2147483647| |99|~|BGCOLOR(seashell):[[max>#g03b20b0]]|R/W (NVM)|0|-2147483648 ~ 2147483647| |100|~|BGCOLOR(seashell):[[time>#g03b20b0]]|R/W (NVM)|0|0 ~ 4294967295| |101|~|BGCOLOR(seashell):[[DetectMode>#g03b20b0]]|R/W (NVM)|0|0 ~ 255| |102|~|BGCOLOR(seashell):[[ProcMode>#g03b20b0]]|R/W (NVM)|0|0 ~ 255| |103|~|BGCOLOR(beige):[[DurationTime>#g03b20b0]]|R|-|0 ~ 4294967295| |104&br;~&br;110|BGCOLOR(seashell):[[MonInfo[1]>#g03b20b0]]|<| | | | |111&br;~&br;117|BGCOLOR(seashell):[[MonInfo[2]>#g03b20b0]]|<| | | | |118&br;~&br;124|BGCOLOR(seashell):[[MonInfo[3]>#g03b20b0]]|<| | | | |125&br;~&br;131|BGCOLOR(seashell):[[MonInfo[4]>#g03b20b0]]|<| | | | *****ID [#ic849fe1] 個体識別番号。~ 0~254の範囲。 *****Baudrate [#l5c8d576] 通信速度[bps]のインデックス。 | Value | Baudrate[bps] |h |CENTER:|LEFT:|c |0| 9600| |1| 19200| |2| 38400| |3| 57600| |4| 100000| |5| 115200| |6| 200000| |7| 230400| |8| 460800| |9| 500000| |10| 1000000| |11| 2000000| |12| 3000000| |13| 4000000| |14| 6000000| *****OffsetAngle [#x7adf8c1] 角度のオフセット値[x100 deg]。~ 内部ではセンサから得られた角度からオフセット値を減じて使用される。 *****LED [#q88f36d9] LEDの点灯・消灯。~ 監視機能の条件一致時のLED点滅が優先される。 | Value | State |h |CENTER:|CENTER:|c |0|OFF| |1|ON| *****ReturnDelayTime [#d2f1f42d] 応答パケットの送信開始前に挿入する遅延時間[ms]。~ 実際にはパケット処理に時間を要し、遅延時間にその処理時間を加えた時間が経過した後に応答する。 *****System Status [#xea777f0] システムステータス。~ |CENTER:|CENTER:|CENTER:|c |Bit|Name|Description|h |31~14|reserve|常時0| |13|Match monitoring conditions|監視一致| |12|Encoder Error|エンコーダ値取得失敗| |11~8|reserve|常時0| |7~0|Encoder Status|エンコーダ値取得時ステータス| *****AngleFlexibility [#a735883f] メインループが角度制御の時、指令角度と現在角度の偏差によってPWM出力の制限を行う。~ n_compliance[x100 deg]<=n_dedzone[x100 deg]<=p_deadzone[x100 deg]<=p_compliance[x100 deg]とする事。 #ref(flexibility.png) deadzoneの間は内部の制御量がリセットされる。 *****CurrentFlexibility [#wd5120f1] メインループが電流制御の時、指令電流と現在電流の偏差によってPWM出力の制限を行う。~ n_compliance[mA]<=n_dedzone[mA]<=p_deadzone[mA]<=p_compliance[mA]とする事。 #ref(flexibility.png) deadzoneの間は制御量がリセットされる。 *****AngleLimit [#f665309d] 角度制御時の[[目標角度>#qce18229]]を内部的にmin~maxの範囲に飽和させる。~ この範囲を超えた目標角度を指令してもエラーにはならない。 *****AngleCtrlGain, VelocityCtrlGain, CurrentCtrlGain [#g5d932a9] 各制御対象におけるPID制御の各ゲイン(x1000)とフィードフォワードゲイン(x1000)、離散積分時間[ms]。~ 各ゲインは1/1000した上で演算に使用される。 | Item | Details |h |CENTER:|LEFT:|c |Kp| x1000 | |Ki| x1000 | |Kd| x1000 | |Kf| x1000 | |delta_t| [ms] | delta_t は各制御対象の固定サンプリング時間のため変更は推奨しない。~ ブロック図は敢えて示さないが、単純なPID演算とカスケード及びフィードフォワード。演算上のオーバーフローなどは一切考慮していないので、極端な数値指定は禁物。 *****TransitionMode [#v5e900e3] [[TransitionTime>#p91011e1]]が1以上の時に目標角度へ遷移するまでの軌跡を指定。 | Value | Transition Mode |h |CENTER:|CENTER:|c |0|定速| |1|S字加減速| |2|S字加速| |3|S字減速| *****TransitionTime [#p91011e1] 最後の角度指令値から目標角度へ遷移するまでの時間[ms]。~ その際の軌跡は[[TansitionMode>#v5e900e3]]による。 *****Excitation [#cff8d1dd] 制御モード。~ 先頭に記述したものがメジャーループ、それ以降に複数記述されているものがマイナーループのカスケード制御となる。PWMについてはいずれの場合においても最終出力段のリミッタとして機能する。 | Value | Control Mode |h |CENTER:|CENTER:|c |0|制御停止(フリー)| |1|Angle (+ PWM)| |2|Velocity (+ PWM)| |3|Angle + Velocity (+ PWM)| |4|Current (+ PWM)| |5|Angle + Current (+ PWM)| |6|Velocity + Current (+ PWM)| |7|Angle + Velocity + Current (+ PWM)| |8|PWM| |14|現在位置で静止(+ PWM)| |15|制御停止(Brake)| *****TargetAngle [#qce18229] 目標角度[x100 deg]。~ 数値を増加させるとサーボの出力軸は時計回りに回転する。 制御時の目標角度は[[AngleLimit>#f665309d]]の上下限値に制限される。~ 動作範囲が限られた機構に適用する場合は、角度センサの中央値である18000(180[deg])を動作範囲の中央位置近傍とする事。 *****TargetVelocity [#mb5bcdb9] 目標回転数[x1000 rot/sec]。~ 数値を増加させるとサーボの出力軸は時計回りに回転する。 *****TargetCurrent [#i34397b2] 目標電流[mA]。~ 数値を増加させるとサーボの出力軸は時計回りに回転する。 *****TargetPWM [#sb6d131e] 目標PWM[‰]。~ 数値を増加させるとサーボの出力軸は時計回りに回転する。~ なおPWMモード以外の制御モードにおいて本PWM値が最終段のリミッタとして機能するため、適切な値を設定した上で使用する事。 *****OffsetTargetAngle [#w7715279] [[目標角度>#qce18229]]に加えて指令される値[x100 deg]。~ 遷移時間を指定して運転している間に偏差を与える場合に使用可。 *****OffsetTargetVelocity [#d3138ca2] [[目標回転数>#mb5bcdb9]]に加えて指令される値[x1000 rot/sec]。~ 遷移時間を指定して運転している間に偏差を与える場合に使用可。 *****OffsetTargetVelocity [#u50c65d4] [[目標電流>#i34397b2]]に加えて指令される値[mA]。~ 遷移時間を指定して運転している間に偏差を与える場合に使用可。 *****CurrentAngle [#eb5380bc] 現在角度[x100 deg]。 *****CurrentVelocity [#n780b169] 現在回転数[x1000 rot/sec]。 *****CurrentCurrent [#lf283fd1] 現在電流[mA] *****CurrentPWM [#g43a4d35] 現在PWM[‰] *****CurrentRawPosition [#c6249941] エンコーダの直値。 *****CurrentBusVoltage [#v51442a1] 印加された電源の電圧[mV]。 *****CurrentMPUTemp [#v4b8fb0d] MPUから取得された温度[x10 degC]。~ 仕様上ではキャリブレーションされているとなっているが、どう見ても未キャリブレーション。 *****CurrentAMPTemp [#taa9d975] アンプから取得された温度[x10 degC]。~ 低インピーダンスなアナログ電圧出力をAD変換しているため、かなり外界に影響されるので参考程度に。 *****CurrentMotorTemp [#ab1de60b] モータ近傍に配したPTCから取得した温度[x10 degC]。~ 標準的な特性曲線から算出しただけなので未キャリブレーション。 *****AngleViation, VelocityViation, CurrentViation [#jd896584] 各制御モードで運転している際の目標値と現在値の偏差。~ この値が大きい時は制御能力を超えた状態を意味する。 *****SysTickTime [#fb9c6462] ファームウェアが実行開始してからの経過時間[ms]。 *****ControlDurationTime [#c7d76d53] Excitationが制御停止以外に設定されてからの経過時間[ms]。 *****NoCommTime [#p75778fa] パケットによる通信が途絶えてからの経過時間[ms]。 *****AngleCorrection [#y925e42a] 16bitのエンコーダ値を角度に変換するためのルックアップテーブル。~ 基準となるエンコーダ値は以下の37箇所で、その時の角度[x100 deg]を設定。~ 0, 1820, 3641, 5461, 7282, 9102,10923,12743,14563,16384,18204,20025,21845,23665,25486,27306,29127,30947,32768,34588,36408,38229,40049,41870,43690,45510,47331,49151,50972,52792,54613,56433,58253,60074,61894,63715,65535~ デフォルトは単純な一次関数で算出した角度[x100 deg]を設定。 なお現状ではセンサそのものが未キャリブレーション状態なのと、一定のPWM値で高速回転させるとセンサのキャリブレーションが働いて自動的に校正されてしまうため、本値を編集する事は推奨しない。 *****MonInfo[0]~MonInfo[4] [#g03b20b0] 異常検出を専用に備えているものではないため、それを補完する目的で装備した監視機能。~ 5つの監視と指定された条件に一致した時の挙動を設定可能。~ |ItemIndex|min/maxと比較するアイテムのインデックス| |min|比較時の最小値| |max|比較時の最大値| |time|DetectModeの一致が継続しProcModeの動作を行うまでの時間| |DetectMode|比較条件&br;0:なし&br;1:min≤value and value≤max&br;2:min≥value or value≥max&br;3:min==value or value==max| |ProcMode|DetectModeの一致が指定時継続した時の処理&br;0:なし&br;1:演算出力 75%制限(制御継続)&br;2:演算出力 50%制限(制御継続)&br;3:演算出力 25%制限(制御継続)&br;4:演算出力 10%制限(制御継続)&br;5:演算出力 0%制限(制御継続)&br;6:フリー(制御停止)&br;7:ストール(再運転不可)| |DurationTime|DetectModeが一致している時間(Read Only)| ProcModeが発動するとLEDは点滅し、DetectModeの条件から外れるとLEDは消灯する(ProcMode=7を除く)。~ ProcModeの0~5が発動している状態でDetectModeの条件が外れると、制限のない制御に復帰する。~ 監視を複数指定した場合、ProcModeの値が大きいものが優先して実行される。 ****Control by serial terminal [#tb971a84] ここではホストとしてPCを使用する場合を紹介する。~ PCにはサーボと通信を行うためのI/Fは装備されていないため、USBポートを介したI/Fを増設して使用する。参考に既成のI/Fを[[こちら>DXLBasicTutorial#pc7cb6ee]]に紹介するが、ここではそのうち[[Dynamixel Starter Kit B>BTH077#f771658c]]に使用されるI/Oボードを使用した。このI/OボードはサーボとのI/Fの他に、供給用のACアダプタ用が接続できるジャックを備えているため、全ての接続がI/Oボードに集中し構成がシンプルになる。なお搭載しているUSBシリアル変換ICの仕様により、最大通信速度が3Mbpsとなる。 #ref(JP200_testbench.png,90%) ファームウェアの解説にある通りサーボはテキストベースの通信プロトコルを備えているため、汎用のシリアル通信ターミナルが使用できる。ここでは[[TeraTerm>http://ttssh2.osdn.jp/]]を使用して操作する事とした。~ TeraTermのシリアルポートの設定は次のように。 #ref(JP200_Teraterm_Portsetting1.png,90%) またPCから送信するパケットを可視化するために、端末のローカルエコーにチェックを入れておく。 #ref(JP200_Teraterm_Termsetting.png) ----- では具体的に通信を行いながら試運転してみる。 手始めに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) しかしこれを手動で行うのは現実的ではないため、[[後述>#a4dfd1bc]]する数値をグラフで表示するプログラム等を用意するのが良いだろう。 ****JP200 test app [#sc4c88c8] 作者自身がパケットを手動で入力するのが面倒になり、テキストボックスに入力したパケットフレームを送るだけのWindowsアプリを作成。 #ref(JP200TestApp.png,70%) 任意のPortとBaudrateを選択してOpenを押せば準備完了。Transmission dataに送信パケットのテキストを入力しエンタキーを押すと送信され、サーボから応答があればReceived dataにその内容が表示される。またウィンドウを拡大すると、受信パケットの内容をコマンド毎に分解して表示する項目が見えるようになる。~ NVMボタンは現在の設定を不揮発メモリに保存、INIボタンは完全初期化、任意のIDを指定して死活確認を行うPing、IDの変更(不揮発化あり)、baudrateの変更(不揮発化無し)する各機能を装備。 サーボのファームウェアは[[ブートローダ>#s6b7645f]]で紹介した操作によって書き換えができる。しかしTeraTermのマクロを使ってもそれ相応に煩雑な面があるので、本アプリのFirmware Update内にある3つのボタンでも書き換えができる(exeファイルと同一フォルダにJP200.binがある場合のみ使用可)。~ #ref(JP200TestApp2.png,65%) 2つ目のタブには4種類の波形で各制御値を更新させながら現在値をグラフで確認する機能を設けている。なお2つのタブは各々異なる方法で通信しているため、ポート名とボーレートは1つめタブの設定を共有するが、開閉は排他的になっている。~ 左上のスライドスイッチでポートの開閉、対象ID・制御モード・遷移時間・波形の周期・波形の種類、各制御の指令値とゲインといった条件や設定が簡易的に試せる。 プロジェクトのReleaseフォルダ内に「jp200_test.exe」として同梱済み。 ****JP200 mon [#a4dfd1bc] teratermや[[jp200_test>#sc4c88c8]]を使っても簡単なテキストによるパケットの送受信を確認するのが限界で、様々な値を試しながら調整を行うには役不足である。~ そこで通信を行いながら各種の情報をリアルタイムにグラフ表示するアプリケーションを用意した。ボーレートやIDの変更・ID指定による検索・初期化や不揮発化・制御モードの選択・ダイアルや数値による各指令値やゲインの変更・任意波形による指令値の更新・モニタ値のスコープチャート表示・任意パケットの送信といった機能を備えている。 > https://www.besttechnology.co.jp/download/jp200_mon.7z #ref(JP200_mon.png,50%) 同梱のjp200_mon.exeは[[LabVIEW>https://www.ni.com/ja/shop/labview.html]]で作られているため、実行するには予めランタイムライブラリをインストールする必要がある。 ****Control by Python [#ta610826] パケットがテキストベースとは言え、シリアルターミナル上で手動による操作は限界がある。ここでは様々なプラットホームで使用できるPythonを使って通信する方法を紹介する。~ ちなみに以下の例は[[RZboard>Tasting the RZBoard V2L]]+[[DXHAT]]の構成をホストとして使用したものである。 まずパケットの送受信は以下のような感じでいかがだろうか。~ ポートとタイムアウト[sec]を指定してpacketクラスを初期化。request関数は指定された条件のパケットを送信し、timeout時間まで「)」が受信されるのを待つ。受信データは「()」で括られた範囲のみを切り出す。複数のサーボを対象にしたパケットの場合はその数だけ受信を繰り返す。requestの引数にpackettype = UNILATERALを指定すると応答無しのパケットを送信、packettype = STRICTを指定するとCRCを付与して送受信。~ おまけでモーションを再生するクラスも追加した。 #html{{ <pre class="brush: python;"; title="jp200class.py"> #! 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 </pre> }} クラスを使って実際にパケットを送ってみる。request関数には括弧を除くパケットの中身を記述、受信データは文字列のlistで受け取る。ここではひとまずデバッグ目的でecho=Trueを指定。 #html{{ <pre class="brush: python;"> 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() </pre> }} 実行するとターミナル上に以下の表示がなされる。 <#1EX=1TA=18000TP=1000> ['#1EX=OKTA=OKTP=OK'] <#1EX=1TA=9000TP=1000> ['#1EX=OKTA=OKTP=OK'] <#1EX=1TA=27000TP=1000> ['#1EX=OKTA=OKTP=OK'] >>> 次に受信したパケットを正規表現任せでコマンド毎に分解してみる。正規表現パターンはクラスに定義済みなのでそれを利用。 #html{{ <pre class="brush: python;"> 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() </pre> }} コマンドと結果がタプルとして表示される。 ('#', '1') ('EX', 'OK') ('TA', 'OK') ('TP', 'OK') ('CA', '17792') ('CV', '0') ('CC', '0') ('CB', '12168') >>> 分離しただけでは意味が無いので、数値変換してID毎にリストに保存してみる。RRコマンドは';'をデリミタとしてデータが連なって返ってくるのでsplitを使って更に分割。 #html{{ <pre class="brush: python;"> 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() </pre> }} [0] 27041 0 -19 1211739 [1] 17892 0 0 1211737 >>> モーションは時間を追って変化する目標値を任意の時間で区切って羅列したデータを予め作っておき、そのデータをサーボの補間機能を活用しながら再生するもの。ここでは2個のサーボを角度制御と速度制御によるモーションを使って運転してみる。 #html{{ <pre class="brush: python;"> 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() </pre> }} 最後にもう一つ。サーボのIDやボーレートを変更する場合はjp200_testで行えるが、Pythonでも変更するコードを紹介しておく。 #html{{ <style type="text/css"> .syntaxhighlighter { overflow-y: auto !important; overflow-x: auto !important; max-height: 900px; -webkit-text-size-adjust: 100%; } </style> <pre class="brush: python;"; title="changeparam.py"> #!/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) </pre> }} ***Issues [#g5d10784] -%%測定値を元に故障や危険を察知して停止させる事は一切行っていない。%%~ 諸条件を監視機能へ設定する事で出力制限をかける事が可能。 -%%電源電圧の変動が大きいと非接触エンコーダが低電圧を検出する事があるが、その場合取得される角度情報の更新が停止する。その状態で制御すると暴走につながるので注意。%%~ 異常検出時はLEDを点滅させ制御を強制停止。 -電源を逆接続すると永久破壊。 -非接触エンコーダは出荷時状態のため、ゼロ位置不定・位置の偏心・速度制御の揺動・運転中の異常検出時にカウント停止が発生する。出力軸のマーカーは何の意味も持たない。 -エンコーダから得られた値をルックアップテーブルを用いて角度にしている。動作範囲が制限される機構(関節等)に適用する場合は、180度(18000)を示す位置と可動範囲の中央が一致するようにする事。通電直後の0度近傍はオーバーフローの危険がある。 -0~360度を超えた角度を指令したり取得する事ができるが、パワーサイクルによって0~360度の範囲にクリップされる。 -半二重通信のドライバとしてGreenPAKを使用しているが、電源の入り切りのタイミングで無用な通信データが排出される。 -通信バスのプルアップがウィークなため、状況によってはブートローダの通信に支障が生じる事がある。ファームウェアの更新の際はプルアップを強化する事を強く推奨する。 -元来電流センサがPWMに対応していないため、電流制御を行う際はリプルが顕著にならないレベルで行う事。 -不揮発データの保存は常時失敗を返すが、それはfspのAPIを使ったデータフラッシュへの書き込み時の戻り値をそのまま返しているからだが、実際には書き込まれている。 -%%割り込み処理内でのI2Cアクセス処理が過大。%% -通信バッファはMPUの大きめなリソース頼みで制限を設けていない。 -各制御値の上下限リミッタが甘い。%%特に長い移動時間と大きい目標値を指定すると簡単に破綻する。%% -温度センサ周辺は理論値によるものでキャリブレーションは行われていない。 -%%プロジェクトの最適化レベルを上げると想定したロジックが破綻するため、苦肉の策としてソースレベルで最適化レベルを変更するようにしている。%% -%%製造済みの実機には古いブートローダが書き込まれているため、ユーザプログラムのXMODEMによる吸い出しは正常に機能しない。%%~ 改修したブートローダで修正済み。 -VBUSの検出端子が他の目的で使用されているため、USBによる運用は推奨されない。 -ここだけの話、TrustZoneの罠とデバッガのバグ等に嵌まってレンガにしたチップは数知れず。いざという時のためにSWD以外にSCIブートモードで使用される端子を引き出せるようにしておくべきのようだ。
JP200(PoC) のバックアップソース(No. All)
現: 2023-10-30 (月) 11:30:14
takaboo
Copyright© 1989,2024 BestTechnology Co.,Ltd.
メニュー
トップ
ニュース
お知らせ
新商品情報
更新情報
マニュアル類
技術情報
チュートリアル
FAQ
お問合せ
技術的なお問合せ
その他お問合せ
お見積り
ブログ
会社案内
ショッピング
ご利用案内
検索
カテゴリ 一覧
ロボットキット
Dynamixelシリーズ
マイコンボード
相撲ロボット
センサ
通信機器
お客様ページ