本記事では、WMXとC# を用いたプログラミング学習の第一歩として、公式サンプルコードの読み解き方を解説します。開発環境はWMX3.6とMicrosoft Visual Studio 2019を想定しております。
前回は、WMX の 開発環境構築 と 起動方法 について解説しました。コンパイルエラーが出た場合は、前回の記事に戻って設定内容や手順を再度ご確認ください。
さて、今回はいよいよ 装置制御 の核となる モーター制御 の具体的な実装に取り組みます。
産業用制御システム や FA (ファクトリーオートメーション) におけるソフトウェアモーションコントロール の基本的な仕組みを理解するために、サンプルコードを用いてモータを動かす方法を解説します。コードの理解を深めるために、重要な箇所には詳細なコメントを追記しています。
[1_BasicMotion]03_MotorControl
こソフトウエアモーションコントロールを用いた、基本的な軸操作のサンプルプログラムについて解説します。 ここでは、以下の3つのステップを実行する制御フローを想定しています。
サーボオン（励磁ON）: モーターにトルクを発生させ、制御可能な状態にする。
座標設定（原点セット）: 現在の位置を「0（または任意の値）」として定義する。
相対移動（Relative Move）: 現在位置を基準に、指定した距離だけ移動する。
サンプルコード
ファイルは以下の位置にあります。
C:\Program Files\SoftServo\WMX3\Samples\CSharp\VS2019\1_BasicMotion\03_MotorControl
通信開始と終了はこちらの記事を参照してください。
using System;
using WMX3ApiCLR;
namespace CSharpAPISample
{
class MotorControl
{
/// <summary>
/// サーボ軸をサーボオンし、軸を動かす方法の説明
/// </summary>
/// <param name="args"></param>
static void Main(string[] args)
{
WMX3Api Wmx3Lib = new WMX3Api();
EngineStatus EnStatus = new EngineStatus();
CoreMotionStatus CmStatus = new CoreMotionStatus();
CoreMotion Wmx3Lib_cm = new CoreMotion(Wmx3Lib);
Console.WriteLine("Program Start.");
System.Threading.Thread.Sleep(1000);
Wmx3Lib.CreateDevice("C:\\Program Files\\SoftServo\\WMX3\\",
DeviceType.DeviceTypeNormal,
0xFFFFFFFF);
Wmx3Lib.SetDeviceName("MotorControl");
// WMX管理下のネットワークに接続します
for (uint i = 0; i < 100; i++)
{
Wmx3Lib.StartCommunication(0xFFFFFFFF);
Wmx3Lib.GetEngineStatus(ref EnStatus);
if (EnStatus.State == EngineState.Communicating)
{
break;
}
}
// サーボネットワーク内のサーボをオン（励磁がONになります）
int axisNo = 0; // 軸番号。０からはじまります
Wmx3Lib_cm.AxisControl.SetServoOn(axisNo, 1); // 1：サーボオン、0：サーボオフ
while (true)
{
Wmx3Lib_cm.GetStatus(ref CmStatus);
if (CmStatus.AxesStatus[0].ServoOn)
{
// サーボONの状態になったときにここを通ります
break;
}
System.Threading.Thread.Sleep(100);
}
// キーを押すまで待機
Console.ReadLine();
// 軸の原点復帰
Config.HomeParam homeParam = new Config.HomeParam();
Wmx3Lib_cm.Config.GetHomeParam(axisNo, ref homeParam);
homeParam.HomeType = Config.HomeType.CurrentPos; // 現在のエンコーダの値を座標０とするモード（このモードは手で軸を動かすと原点がずれます）
//homeParam.HomeType = Config.HomeType.HS; // 原点センサの位置まで、位置のサーチを行うモードです
Wmx3Lib_cm.Config.SetHomeParam(axisNo, homeParam);
Wmx3Lib_cm.Home.StartHome(axisNo); // 現在位置を座標０にします
Wmx3Lib_cm.Motion.Wait(axisNo);
// モーションプロファイルを指定 軸の移動はこのように移動パラメータをクラスにセットします。
Motion.PosCommand posCommand = new Motion.PosCommand();
// 矩形プロファイルで動作：速度が目標速度に等しくなるまで、軸が一定の加速度で加速します。軸が目標位置に近づくと、軸は目標に正確に停止するタイミングで一定の減速度で減速を開始します。
posCommand.Profile.Type = WMX3ApiCLR.ProfileType.Trapezoidal;
posCommand.Axis = axisNo;
posCommand.Target = 1000000; // 目的位置の設定
posCommand.Profile.Velocity = 100000;
posCommand.Profile.Acc = 1000000;
posCommand.Profile.Dec = 1000000;
// 相対位置として指定する位置指令を開始します。
Wmx3Lib_cm.Motion.StartMov(posCommand); // 現在位置から1000000移動します。
//Wmx3Lib_cm.Motion.StartPos(posCommand); // 絶対位置へ移動する場合はこちらの命令になります。
//モーションが終了するまで実行をブロッキングします。指令が終わった時点で抜けます。位置決め完了を待つわけではありません。
Wmx3Lib_cm.Motion.Wait(axisNo);
// キーを押すまで待機
Console.ReadLine();
// サーボネットワーク内のサーボをオフ
Wmx3Lib_cm.AxisControl.SetServoOn(axisNo, 0); // 1：サーボオン、0：サーボオフ
while (true)
{
Wmx3Lib_cm.GetStatus(ref CmStatus);
if (!CmStatus.AxesStatus[0].ServoOn)
{
break;
}
System.Threading.Thread.Sleep(100);
}
// WMX管理下のネットワーク通信を終了します
Wmx3Lib.StopCommunication(0xFFFFFFFF);
Wmx3Lib.CloseDevice(); // StopCommunicationを先に行ってください
Wmx3Lib_cm.Dispose();
Wmx3Lib.Dispose();
Console.WriteLine("Program End.");
System.Threading.Thread.Sleep(3000);
}
}
}実行結果
軸の制御と状態は、WMXの標準ツールConsoleTOOLでも確認することができます。自作アプリと並行で使うことができますので、デバック時には起動しておくことをお勧めいたします。
サーボONの時の座標
StartMovを行った時の座標（Ｃ＃コードを実行した後）
軸の原点復帰について
原点復帰の動作はConfig.HomeTypeで指定します。物理的な使用に合わせて変更してください。良く使用するモードをご説明いたします。
CurrentPosは 現在のエンコーダの値を座標０とするモード
基本的に装置で使うことはないと思います。このモードは手で軸を動かすと原点がずれます。
HS 原点復帰方向にホームスイッチを探すモード
装置に使うには、この方法が多いと思います。
HSOffZPulseホームスイッチの立ち下がりエッジを探し、ホームスイッチの立ち下がりエッジ後の最初のZ相（インデックスパルス）を探します。
モータのZ相を原点として使用します。原点位置の再現性が高いです。
LS 軸は原点復帰方向のリミットスイッチ
リミットスイッチを原点として使用します。原点センサを別途用意しなくても、基準の位置を検出ことができます。
[1_BasicMotion]04_MotorStop
モーションを実行している軸を、停止させる方法の説明です。
コードのStartJog関数は、モータを連続的に回転させる命令です。止める命令を送るまで周り続けます。回し続ける軸は、シングルターンモードを検討してください。
サンプルコード
C:\Program Files\SoftServo\WMX3\Samples\CSharp\VS2019\1_BasicMotion\04_MotorStop
サンプルコードに手をいれて、ExecQuickStopのコードを独自に追加しています。また、連続で動作させるために、コンソールでの動作切替も変更しています。
/*****************************************************************************/
/* FILE : MotorStop.cs */
/* DESCRIPTION : Sample to stop servo by four methods */
/*****************************************************************************/
using System;
using WMX3ApiCLR;
namespace CSharpAPISample
{
class MotorStop
{
/*-------------------------------------------------------------------*/
/* Function : _tmain */
/* Description : Main Function. */
/*-------------------------------------------------------------------*/
static void Main(string[] args)
{
WMX3Api Wmx3Lib = new WMX3Api();
EngineStatus EnStatus = new EngineStatus();
CoreMotionStatus CmStatus = new CoreMotionStatus();
CoreMotion Wmx3Lib_cm = new CoreMotion(Wmx3Lib);
Console.WriteLine("Program Start.");
System.Threading.Thread.Sleep(1000);
Wmx3Lib.CreateDevice("C:\\Program Files\\SoftServo\\WMX3\\",
DeviceType.DeviceTypeNormal,
0xFFFFFFFF);
Wmx3Lib.SetDeviceName("MotorStop");
for (uint i = 0; i < 100; i++)
{
Wmx3Lib.StartCommunication(0xFFFFFFFF);
Wmx3Lib.GetEngineStatus(ref EnStatus);
if (EnStatus.State == EngineState.Communicating)
{
break;
}
}
int axisNo = 0; // 軸番号。０からはじまります
Wmx3Lib_cm.AxisControl.SetServoOn(axisNo, 1);
while (true)
{
Wmx3Lib_cm.GetStatus(ref CmStatus);
if (CmStatus.AxesStatus[axisNo].ServoOn)
{
break;
}
System.Threading.Thread.Sleep(100);
}
// キーを押すまで待機
Console.ReadLine();
// 軸の原点復帰
Config.HomeParam homeParam = new Config.HomeParam();
Wmx3Lib_cm.Config.GetHomeParam(axisNo, ref homeParam);
homeParam.HomeType = Config.HomeType.CurrentPos;
Wmx3Lib_cm.Config.SetHomeParam(axisNo, homeParam);
Wmx3Lib_cm.Home.StartHome(axisNo);
Wmx3Lib_cm.Motion.Wait(axisNo);
// モーションプロファイルを指定
Motion.JogCommand jogCommand = new Motion.JogCommand();
jogCommand.Profile.Type = WMX3ApiCLR.ProfileType.Trapezoidal;
jogCommand.Axis = axisNo;
jogCommand.Profile.Velocity = 100000;
jogCommand.Profile.Acc = 1000000;
jogCommand.Profile.Dec = 1000000;
// Quick Stopの停止パラメータ設定
var motionParam = new Config.MotionParam();
Wmx3Lib_cm.Config.GetMotionParam(axisNo, ref motionParam); // 軸の設定値を取得
motionParam.QuickStopDec = 10000; // クイックストップを実行したときに軸を停止させる減速度[ユーザー単位 / 秒^2 ]
Wmx3Lib_cm.Config.SetMotionParam(axisNo, motionParam); // 軸の設定値を設定
// 自動で全メソッドを処理するように変更しました
string[] message = new string[] {
"1.Normal Stop",
"2.Certain time Stop(10ms)",
"3.Quick Stop",
"4.Deceleration Stop",
};
for (int i = 1; i <= 4; i++)
{
Console.WriteLine(message[i-1]);
// モータを連続的に回転させます
Wmx3Lib_cm.Motion.StartJog(jogCommand);
System.Threading.Thread.Sleep(500);
switch (i)
{
case 2: // Certain time Stop(10ms).
Console.WriteLine("Certain time Stop(10ms).");
Wmx3Lib_cm.Motion.ExecTimedStop(axisNo, 10); // 10ms
break;
case 3: // Quick Stop.
Console.WriteLine("Quick Stop.");
Wmx3Lib_cm.Motion.ExecQuickStop(axisNo);
break;
case 4: // Deceleration Stop.
Console.WriteLine("Deceleration Stop.");
Wmx3Lib_cm.Motion.Stop(axisNo, 1000000);
break;
case 1: // Normal Stop.
default:
Console.WriteLine("Normal Stop.");
Wmx3Lib_cm.Motion.Stop(axisNo);
break;
}
//モーションが終了するまで実行をブロッキングします。指令が終わった時点で抜けます。
Wmx3Lib_cm.Motion.Wait(axisNo);
System.Threading.Thread.Sleep(500);
}
// サーボネットワーク内のサーボをオフ
Wmx3Lib_cm.AxisControl.SetServoOn(axisNo, 0);
while (true)
{
Wmx3Lib_cm.GetStatus(ref CmStatus);
if (!CmStatus.AxesStatus[axisNo].ServoOn)
{
break;
}
System.Threading.Thread.Sleep(100);
}
Wmx3Lib.StopCommunication(0xFFFFFFFF);
Wmx3Lib.CloseDevice();
Wmx3Lib_cm.Dispose();
Wmx3Lib.Dispose();
Console.WriteLine("Program End.");
System.Threading.Thread.Sleep(3000);
}
}
}
実行結果
表は赤色が座標で、オレンジ色が速度です。
１がStop命令で、Profile.Decの値で減速停止します
２がExecTimedStop命令で、指定msの時間で停止します。
3がExecQuickStop命令で、指定の減速度で停止します。パラメータは motionParam.QuickStopDec = 10000; で指定します。
4がStop命令で、プロファイルは無視して引数の値で減速停止します
停止させるにも、複数の命令があります。使用するアプリケーションに合わせて使ってください。
[1_BasicMotion]05_MotorStatus
軸のステータスを取得する方法を説明します。
サンプルコード
相対位置に移動を開始し、１のキー入力で軸の状態を画面に表示します。 実機では、サーボエラーやリミットに到達を確認しながら処理を行います。
using System;
using WMX3ApiCLR;
namespace CSharpAPISample
{
class MotorStatus
{
/// <summary>
/// 軸のステータスを取得する方法
/// </summary>
/// <param name="args"></param>
static void Main(string[] args)
{
WMX3Api Wmx3Lib = new WMX3Api();
EngineStatus EnStatus = new EngineStatus();
CoreMotionStatus CmStatus = new CoreMotionStatus();
CoreMotion Wmx3Lib_cm = new CoreMotion(Wmx3Lib);
Console.WriteLine("Program Start.");
System.Threading.Thread.Sleep(1000);
Wmx3Lib.CreateDevice("C:\\Program Files\\SoftServo\\WMX3\\",
DeviceType.DeviceTypeNormal,
0xFFFFFFFF);
Wmx3Lib.SetDeviceName("MotorStatus");
for (uint i = 0; i < 100; i++)
{
Wmx3Lib.StartCommunication(0xFFFFFFFF);
Wmx3Lib.GetEngineStatus(ref EnStatus);
if (EnStatus.State == EngineState.Communicating)
{
break;
}
}
Wmx3Lib_cm.AxisControl.SetServoOn(0, 1);
while (true)
{
Wmx3Lib_cm.GetStatus(ref CmStatus);
if (CmStatus.AxesStatus[0].ServoOn)
{
break;
}
System.Threading.Thread.Sleep(100);
}
Config.HomeParam homeParam = new Config.HomeParam();
Wmx3Lib_cm.Config.GetHomeParam(0, ref homeParam);
homeParam.HomeType = Config.HomeType.CurrentPos;
Wmx3Lib_cm.Config.SetHomeParam(0, homeParam);
Wmx3Lib_cm.Home.StartHome(0);
Wmx3Lib_cm.Motion.Wait(0);
Motion.PosCommand posCommand = new Motion.PosCommand();
posCommand.Profile.Type = WMX3ApiCLR.ProfileType.Trapezoidal;
posCommand.Axis = 0;
posCommand.Target = 1000000;
posCommand.Profile.Velocity = 100000;
posCommand.Profile.Acc = 1000000;
posCommand.Profile.Dec = 1000000;
// 相対移動の開始
Wmx3Lib_cm.Motion.StartMov(posCommand);
// Dump data.
while (true)
{
Console.WriteLine("1:status dump else:end\n");
Console.WriteLine("->");
int selectNum;
while (true)
{
string line = Console.ReadLine();
if (line != null && int.TryParse(line, out selectNum))
{
break;
}
Console.WriteLine("->");
}
if (selectNum != 1) // 数値の１以外でプログラム終了
{
break;
}
//---------------------------------------------------------------
// Dump information on specified axis.
//---------------------------------------------------------------
Wmx3Lib_cm.GetStatus(ref CmStatus);
CoreMotionAxisStatus cmAxis = CmStatus.AxesStatus[0];
Console.WriteLine(" 1.PosCmd :" + cmAxis.PosCmd.ToString("F3")); // 1. PosCmd
Console.WriteLine(" 2.ActPos :" + cmAxis.ActualPos.ToString("F3")); // 2. ActualPos
Console.WriteLine(" 3.ActVel :" + cmAxis.ActualVelocity.ToString("F3")); // 3. ActualVelocity
Console.WriteLine(" 4.ActTrq :" + cmAxis.ActualTorque.ToString("F3")); // 4. ActualTorque
Console.WriteLine(" 5.AmpAlm :" + cmAxis.AmpAlarm.ToString()); // 5. AmpAlarm
Console.WriteLine(" 6.AmpAlmCode:" + cmAxis.AmpAlarmCode.ToString("X5")); // 6. AmpAlarmCode
Console.WriteLine(" 7.SrvOn :" + cmAxis.ServoOn.ToString()); // 7. ServoOn
Console.WriteLine(" 8.HomeDone :" + cmAxis.HomeDone.ToString()); // 8. HomeDone
Console.WriteLine(" 9.InPos :" + cmAxis.InPos.ToString()); // 9. InPos
Console.WriteLine("10.NegLS :" + cmAxis.NegativeLS.ToString()); // 10.negativeLS
Console.WriteLine("11.PosLS :" + cmAxis.PositiveLS.ToString()); // 11.positiveLS
Console.WriteLine("12.HomeSw :" + cmAxis.HomeSwitch.ToString()); // 12.homeSwitch
Console.WriteLine("13.OpState :" + OP_STATE(cmAxis.OpState)); // 13.OperationState
}
Wmx3Lib_cm.Motion.ExecQuickStop(0);
Wmx3Lib_cm.Motion.Wait(0);
Wmx3Lib_cm.AxisControl.SetServoOn(0, 0);
while (true)
{
Wmx3Lib_cm.GetStatus(ref CmStatus);
if (!CmStatus.AxesStatus[0].ServoOn)
{
break;
}
}
// Stop Communication.
Wmx3Lib.StopCommunication(0xFFFFFFFF);
//Quit device.
Wmx3Lib.CloseDevice();
Wmx3Lib_cm.Dispose();
Wmx3Lib.Dispose();
Console.WriteLine("Program End.");
System.Threading.Thread.Sleep(3000);
}
/// <summary>
/// 軸の状態を文字で取得
/// </summary>
/// <param name="state">軸の状態</param>
/// <returns>文字</returns>
private static string OP_STATE(WMX3ApiCLR.OperationState state)
{
switch (state)
{
case WMX3ApiCLR.OperationState.Idle:
return "Idle";
case WMX3ApiCLR.OperationState.Pos:
return "Pos";
case WMX3ApiCLR.OperationState.Jog:
return "Jog";
case WMX3ApiCLR.OperationState.Home:
return "Home";
case WMX3ApiCLR.OperationState.Sync:
return "Sync";
case WMX3ApiCLR.OperationState.GantryHome:
return "GantryHome";
case WMX3ApiCLR.OperationState.Stop:
return "Stop";
case WMX3ApiCLR.OperationState.Intpl:
return "Intpl";
case WMX3ApiCLR.OperationState.ConstLinearVelocity:
return "ConstLinearVelocity";
case WMX3ApiCLR.OperationState.Trq:
return "Trq";
case WMX3ApiCLR.OperationState.DirectControl:
return "DirectControl";
case WMX3ApiCLR.OperationState.PVT:
return "PVT";
case WMX3ApiCLR.OperationState.ECAM:
return "ECAM";
case WMX3ApiCLR.OperationState.SyncCatchUp:
return "SyncCatchUp";
case WMX3ApiCLR.OperationState.DancerControl:
return "DancerControl";
default:
return "";
}
}
}
}
実行結果
実行したすると、結果文字が出力されます。
1.PosCmd：指令の座標です。
2.ActualPos：フィードバック位置（エンコーダの現在位置）
3.ActualVelocity：フィードバックの速度
4.ActualTorque:軸のフィードバックトルク
5.AmpAlarm：アンプがアラーム状態にある場合はtrueになります
6.AmpAlarmCode:アンプアラームコード
7.ServoOn:True=励磁している状態
8.HomeDone:TRUE：TRUE：原点復帰が完了
9.InPos:位置決め完了フラグ 位置決めフラグの設定はこちらを参照してください
10.NegativeLS:TRUE=負方向の近接リミットスイッチがオン
11.PositiveLS:TRUE=正方向のリミットスイッチがオン
12.HomeSwitch:TRUE=ホームスイッチがオン
13.OpState:軸の動作状態
コラム サーボOFF（脱力）状態でも、コントローラーは位置を見失わない
通常、ステッピングモーターなどのオープンループ制御では、励磁を切ると位置情報が信頼できなくなります。しかし、エンコーダー（位置検出器）付きのサーボモーターを使用している場合、ソフトウエアモーションコントローラは常にエンコーダーからのフィードバック値を監視し続けています。
これにより、以下のような柔軟な運用（ユースケース）が可能になります。
- 手動介入（ティーチングなど）：
- プログラムで
ServoOffを実行し、軸をフリーにする。 - 作業者が手で可動部（軸）を動かし、任意の位置へ調整する。
- コントローラーはその間の移動量をカウントし続けているため、座標ズレは発生しない。
- 再度
ServoOnし、そこからプログラムによる自動運転を再開する。
- プログラムで
これは、段取り替えやメンテナンス、簡易的なティーチング機能を実装する際に非常に有効な手段です。
おわりに
以上で、基本的なモーション制御である「移動」「停止」「状態（ステータス）取得」の実装が完了しました。
この手法の最大の利点は、複雑なフィールドバス通信やサーボアンプとのやり取りが、APIによって完全に抽象化（隠蔽）されている点です。 プログラマーは、通信パケットの生成やタイミング制御といった下位レイヤーの処理を意識する必要がありません。そのため、本来注力すべき「装置のアプリケーションロジック」や「ユーザーインターフェース」の開発にリソースを集中することができます。
この記事のC#コードは、Windowsスレッドのため命令と命令の時間のばらつきはあります。そのため装置が止まらないように動かすには無理があります。リアルタイムで処理をおこなう必要がある場合はこちらの記事を参考にしてください。
この記事が、これからWMXを始める方のお力になれば幸いです。
コメント