サーボの位置決め完了とは
サーボでは指令に遅れてモータが遅れてついてくることになります。指令の送信完了で軸が目的地に到達しているわけではありません。 この記事ではソフトで軸の移動完了を判定する処理をご紹介いたします。
WMXの位置決め完了とは
指令とエンコーダーの差分が、設定値(InPosWidth)を超えていたら場合はfalseとなります。範囲内であればtrueになります。ただし、InPosWidthを設定しても、フラグが変化するのみでモーションの動きが変わりません。
実際のコードで位置決め完了を待つ場合は、モーション指令を送った後に、InPosフラグをポーリングで確認し続ける必要があります。範囲内に入ったところで、次の動作指令を送信します。
オーバーシュート時の注意点
軸がオーバシュートしている時は、InPosがONとOFFを繰り返すことになります。（目標よりも行き過ぎて戻っている状態）
WMXには、オーバーシュート時の位置決め完了処理関数はありませんので、ユーザーがアプリケーションに合わせて作成する必要があります。
モーション間の位置決め完了幅の決め方
きつく（狭く）すると
モーション間のつなぎ目できっちり止まるようになります。モーション全体でみると時間が伸びてしまいます。
ゆるく（広く）すると
あるモーションの終了間際から、次のモーションにつなげた動作になります。（ブレンド補間）
経由でよい場合は、余分な待ち時間を軽減しサイクルタイムが改善します。
コードサンプル
位置決め完了幅の設定
※WMX3.6で検証したコード
/// <summary>
/// 位置決め完了幅の設定
/// </summary>
/// <remarks>
/// 実行中のモーション指令の目標位置を中心とする範囲の幅。フィードバック位置がこの範囲内にある場合、軸はインポジションであるとみなされます。
/// </remarks>
/// <param name="inPosWidth">位置決め完了幅 1つ目</param>
/// <param name="inPosWidth2">位置決め完了幅 2つ目</param>
/// <param name="inPosWidth3">位置決め完了幅 3つ目</param>
///
public void SetInPosWidth(int AxisNo, double inPosWidth, double inPosWidth2, double inPosWidth3)
{
// 現在の値を読み込みます
Config.FeedbackParam FeedbackParam = new Config.FeedbackParam();
CMotion.Config.GetFeedbackParam(AxisNo, ref FeedbackParam);
// 位置決め完了幅のみ書き換えます
FeedbackParam.InPosWidth = inPosWidth; // 実行中のモーション指令の目標位置を中心とする範囲の幅 1つ目
FeedbackParam.InPosWidth2 = inPosWidth2; // 実行中のモーション指令の目標位置を中心とする範囲の幅 2つ目
FeedbackParam.InPosWidth3 = inPosWidth3; // 実行中のモーション指令の目標位置を中心とする範囲の幅 3つ目
CMotion.Config.SetFeedbackParam(AxisNo, FeedbackParam);
}// おまじない(制御クラスの初期化)
//API = new WMX3Api();
//CMotion = new CoreMotion(API);
//API.CreateDevice("C:\\Program Files\\SoftServo\\WMX3\\", DeviceType.DeviceTypeNormal, 0xFFFFFFFF); // WMXに接続
//API.StartCommunication(0xFFFFFFFF); // 通信開始
int 制御対象軸 = 2;
// 現在位置を0にする
CMotion.Home.SetCommandPos(制御対象軸, 0); // 現在位置のクリア
// 位置決め完了幅の設定
SetInPosWidth(制御対象軸, 10, 300, 600);インポジションフラグの確認（検証）
指令は０のままで軸を手で回し、どのように位置決め完了フラグが変わるのかを検証しました。
int 制御対象軸 = 2;
// 値の取得
var coreMotionStatus = new CoreMotionStatus();
CMotion.GetStatus(ref coreMotionStatus); // 軸の状態取得
double フィードバック位置 = coreMotionStatus.AxesStatus[制御対象軸].ActualPos;
double 指令位置 = coreMotionStatus.AxesStatus[制御対象軸].PosCmd;
// 位置決め完了幅の取得
// TRUE：軸は実行中のモーション指令の目標位置のIn Pos Width幅以内にあります。
// FALSE：軸は実行中のモーション指令の目標位置のIn Pos Width幅以内にありません。
Console.WriteLine(Environment.NewLine + "指令位置:" + 指令位置 + "フィードバック:" + フィードバック位置.ToString("0"));
Console.WriteLine("InPos:" + coreMotionStatus.AxesStatus[制御対象軸].InPos);
Console.WriteLine("InPos2:" + coreMotionStatus.AxesStatus[制御対象軸].InPos2);
Console.WriteLine("InPos3:" + coreMotionStatus.AxesStatus[制御対象軸].InPos3); InPosWidth=10 InPosWidth2=300 InPosWidth3=600 としたときのコードの実行結果は意図したものとなりました。
指令位置:0フィードバック:3
InPos:True
InPos2:True
InPos3:True
指令位置:0フィードバック:289
InPos:False
InPos2:True
InPos3:True
指令位置:0フィードバック:363
InPos:False
InPos2:False
InPos3:True
指令位置:0フィードバック:804
InPos:False
InPos2:False
InPos3:False位置決め完了を待つ場合のポーリング処理サンプル
ポーリングで待機を行います。 オーバーシュートが落ち着くまで待ちたい場合は、「一定時間InPosがONである」などのロジックをユーザーで実装する必要があります。※WMX3.6で検証したコード
// この時点で指令が送られた状態
while (true)
{
CMotion.GetStatus(ref coreMotionStatus); // 軸の状態を取得
if (coreMotionStatus.AxesStatus[制御対象軸].InPos) // 位置決め完了幅の１つ目で判定
{
// In Pos Width幅以内まで移動したとき
break;
}
System.Threading.Thread.Sleep(100); // ポーリング時間
}
// この時点で到達していますが、軸がオーバーシュートしている可能性があります
コメント