C#コントロールプログラム編 
Arduino互換機でL6470のデイジーチェインを試してみる Arduinoのスケッチと接続編のつづき
スッテッピングモーターの用途次第だが
必要な動きが決まっていればPCと接続しないでArduino側で制御すればよい。
全コマンドの挙動を確かめるのと個々のデバイスへ同時にコマンド送信して、
回転制御とレジスタ値を取得したいので、これらすべてPC側のソフトで処理した。
今回、初期パラメーターはストロベリーリナックスのマニュアル記載通りに設定した。
モーター駆動電源はATX電源を再利用したものから１２Vを使った。
別のステッピングモーターを制御する場合や駆動電圧変更など
条件変化に伴うパラメーター調整は、PCから直接設定を変更しながら
動きを確認できるので、パラメーターの最適値を探すときに役立つと思う。
面倒なことを処理する部分をクラスにしてみた。
スッテッピングモーターの用途次第だが
必要な動きが決まっていればPCと接続しないでArduino側で制御すればよい。
全コマンドの挙動を確かめるのと個々のデバイスへ同時にコマンド送信して、
回転制御とレジスタ値を取得したいので、これらすべてPC側のソフトで処理した。
今回、初期パラメーターはストロベリーリナックスのマニュアル記載通りに設定した。
モーター駆動電源はATX電源を再利用したものから１２Vを使った。
別のステッピングモーターを制御する場合や駆動電圧変更など
条件変化に伴うパラメーター調整は、PCから直接設定を変更しながら
動きを確認できるので、パラメーターの最適値を探すときに役立つと思う。
面倒なことを処理する部分をクラスにしてみた。
CmdPropertyクラス
ひとつのデバイスに関する文字列コマンドをバイトデータに変換する
- プロパティ
public int[] Binary { get; private set; } // 最終的なコマンドをバイナリーに変換した結果
public string Command { get; set; } // コマンド
public string Register { get; set; } // レジスター
public string Direction { get; set; } // 回転方向
public string Action { get; set; } // アクション
public int Value { get; set; } // 設定値
public int xBin { get; private set; } // バイナリのバイト数
public int xRes { get; private set; } // レスポンスを要求するバイト数
public bool IsOk { get; private set; } // バイナリーが完成しているか
- メンバー関数
public CmdProperty() // コンストラクタ：int[]Binaryを1バイト確保 public bool SetBinary() // バイトデータに変換、変換に成功すればTureを返す private void ArrayAdjustment() // int[]Binaryをコマンドに必要な大きさに確保し直す private void SetValue(int mask) // レジスタの最大ビット数を超えないようマスクしてからint[]Binaryに格納する
DaisyChainクラス
チェイン数にあわせて送信データを並び替える／受信データをデバイスごとに整理する
- プロパティ
public CmdProperty[] CmdSet { get; set; } // CmdPropertyクラスの配列
public byte[] SendData { get; private set; }// 並び替えた送信データ
public int xSend { get; private set; }// SendDataのバイト数
public int xDevice { get; private set; }// チェイン数
public int xRecive { get; private set; }// レスポンス要求バイト数（必要な送り出しバイト数）
public byte[,] ReciveData { get; private set; }// レスポンスをデバイスごとに整理したバイナリ
public string[,] strRecive { get; private set; }// ReciveDataを16進数の文字列に変換
- メンバー関数
public DaisyChain(int DeviceCount) // コンストラクタ：CmdProperty[] CmdSetをチェイン数確保 public bool GetSendData() // 送信データの並び替え public bool FixReciveData(byte[] data) // 受信データの並び替え
使い方
- イニシャライズとチェイン数の取得
ArduinoはスケッチのL6470_Initialize()を実行。
L6470_Initialize()はL6470のチェイン数をカウントし
すべてのL6470デバイスリセットと初期値を設定した後にPCへチェイン数を返す。
byte[] data = new byte[2] { 0x01, 0xFF };
serialPort1.Write(data, 0, 2);
送信後シリアルポートが受信する１バイトにチェイン数が数値(文字列でない)で返ってくる。
- DaisyChainクラスは先に取得したチェイン数を渡してインスタンスをつくる。
この場合は２台がデイジーチェインされているとき
- 送信
device1に軸位置を問い合わせる
DC.CmdSet[0].Command = "GetParam";
DC.CmdSet[0].Register = "ABS_POS";
DC.CmdSet[0].SetBinary();
// device1のコマンドコードがセットされる DC.CmdSet[0].Binaryは[1]{0x21}
device2はスピード0xFFFFで右回転させる
DC.CmdSet[1].Command = "Run";
DC.CmdSet[1].Direction = "Foward";
DC.CmdSet[1].Value = 0xffff;
DC.CmdSet[1].SetBinary();
// device2のコマンドコードがセットされる DC.CmdSet[1].Binaryは[4]{0x51, 0x00, 0xff, 0xff}
送信データの並び替え
DC.GetSendData();
// 並び換えられた送信データ DC.SendDataは[10]{0x09, 0x06, 0x51, 0x00, 0x00, 0x00, 0xff, 0x00, 0xff, 0x21}
serialPort1.Write(DC.SendData, 0, DC.xSend); // シリアルポートからArduinoに送信
- 受信
// 上記の場合 dataは[6]{0x00, MSB, 0x00, 0x??, 0x00, LSB}
DC.FixReciveData(data);
// DC.ReciveDataにデバイスのレジスタが格納
// device2からの返信はないが送り出しに使われた0x00が格納される
// DC.ReciveData[0, 3]は{MSB, 0x??, LSB}
// DC.ReciveData[1, 3]は{0x00, 0x00, 0x00}
// DC.strRecive[0]と[1]に16進数文字列が格納
}
serialPort1のデリゲートと受信イベントの記述の例
private delegate void DataReceivedHandler(byte[] data);
private void serialPort1_DataReceived(object sender, SerialDataReceivedEventArgs e) {
if (!serialPort1.IsOpen) return;
try {
byte[] data = new byte[serialPort1.BytesToRead];
serialPort1.Read(data, 0, data.GetLength(0));
Invoke(new DataReceivedHandler(reciveDataWrite), new Object[] { data });
}
catch (IOException ex) { MessageBox.Show(ex.Message); }
}
L6470.cs
using System;
namespace L6470
{
// L6470定数定義
enum RegAddress : int // レジスタアドレス
{
ABS_POS = 1,
EL_POS,
MARK,
SPEED,
ACC,
DEC,
MAX_SPEED,
MIN_SPEED,
KVAL_HOLD,
KVAL_RUN,
KVAL_ACC,
KVAL_DEC,
INT_SPD,
ST_SLP,
FN_SLP_ACC,
FN_SLP_DEC,
K_THERM,
ADC_OUT,
OCD_TH,
STALL_TH,
FS_SPD,
STEP_MODE,
ALARM_EN,
CONFIG,
STATUS
}
enum RegLength : int // レジスター長（bit数）
{
ABS_POS = 22,
EL_POS = 9,
MARK = 22,
SPEED = 20,
ACC = 12,
DEC = 12,
MAX_SPEED = 10,
MIN_SPEED = 13,
KVAL_HOLD = 8,
KVAL_RUN = 8,
KVAL_ACC = 8,
KVAL_DEC = 8,
INT_SPD = 14,
ST_SLP = 8,
FN_SLP_ACC = 8,
FN_SLP_DEC = 8,
K_THERM = 4,
ADC_OUT = 5,
OCD_TH = 4,
STALL_TH = 7,
FS_SPD = 10,
STEP_MODE = 8,
ALARM_EN = 8,
CONFIG = 16,
STATUS = 16
}
enum Commands : int // コマンドコード
{
Nop,
SetParam = 0x00,
GetParam = 0x20,
Run = 0x50,
StepClock = 0x58,
Move = 0x40,
GoTo = 0x60,
GoTo_DIR = 0x68,
GoUntil = 0x82,
ReleseSW = 0x92,
GoHome = 0x70,
GoMark = 0x78,
ResetPos = 0xD8,
ResetDevice = 0xC0,
SoftStop = 0xB0,
HardStop = 0xB8,
SoftHiZ = 0xA0,
HardHiZ = 0xA8,
GetStatus = 0xD0
}
enum CmdLength : int // コマンド長（バイト数）
{
Nop = 1,
GetParam = 1,
Run = 4,
StepClock = 1,
Move = 4,
GoTo = 4,
GoTo_DIR = 4,
GoUntil = 4,
ReleseSW = 1,
GoHome = 1,
GoMark = 1,
ResetPos = 1,
ResetDevice = 1,
SoftStop = 1,
HardStop = 1,
SoftHiZ = 1,
HardHiZ = 1,
GetStatus = 1
}
public class CmdProperty // デバイスに対するコマンド情報と計算
{
public int[] Binary { get; private set; } // 最終的なコマンドをバイナリーに変換した結果
public string Command { get; set; } // コマンド
public string Register { get; set; } // レジスタ
public string Direction { get; set; } // 回転方向
public string Action { get; set; } // アクション
public int Value { get; set; } // 設定値
public int xBin { get; private set; } // バイナリのバイト数
public int xRes { get; private set; } // レスポンスを要求するバイト数
public bool IsOk { get; private set; } // バイナリーが完成しているか
public CmdProperty() { Binary = new int[1]{0}; } // コンストラクタ：バイナリをとりあえず１バイトだけ確保
private void ArrayAdjustment()
{
Binary = null;
Binary = new int[xBin];
}
private void SetValue(int masked)
{
masked &= Value;
for (int i = xBin - 1; i > 0; i--) {
Binary[i] = masked & 0xFF;
masked >>= 8;
}
}
public bool SetBinary()
{
int nowLength = Binary.Length;
IsOk = false;
if (Command.Equals("SetParam")) // コマンドがSetParamの場合
{ // SetParamは対象Regによりコマンド長が可変する
if (string.IsNullOrEmpty(Register)) return false; // RegisterがSetされているか
try { xRes = (int)(Enum.Parse(typeof(RegLength), Register)); } // レジスターのビット数を取得
catch { return false; }
xBin = (int)Math.Ceiling(xRes / 8.0) + 1; // xBinに必要なコマンド長を計算
if (nowLength != xBin) ArrayAdjustment(); // Binaryを必要なバイト数に確保し直す
try { Binary[0] = (int)(Enum.Parse(typeof(RegAddress), Register)); } // 先頭バイトにコマンドを設定
catch { return false; } // コマンドはReg Adr=PARAM → 先頭バイトのBit0〜4 / Bit5〜7は0x00
SetValue((int)Math.Pow(2, xRes) - 1); // レジスターのビット数で許容できる範囲をマスクして２バイト以降に設定する
xRes = 0; // レスポンスは要求しないのでxRes = 0
return IsOk = true; // 完成フラグを設定してリターン
}
try { xBin = (int)(Enum.Parse(typeof(CmdLength), Command)); } // xBinに必要なコマンド長を取得
catch { return false; }
if (nowLength != xBin) ArrayAdjustment(); // Binaryを必要なバイト数に確保し直す
try { Binary[0] = (int)(Enum.Parse(typeof(Commands), Command)); } // 先頭バイトにコマンドを設定
catch { return false; }
if (Command.Equals("GetParam")) // コマンドがGetParamの場合
{
if (string.IsNullOrEmpty(Register)) return false; // 対象レジスターが登録されているか
try { Binary[0] = (int)(Enum.Parse(typeof(RegAddress), Register)) | 0x20; } // 先頭バイトにコマンドを設定
catch { return false; } // コマンドはReg Adr=PARAM → 先頭バイトのBit0〜4 + 0x20 / Bit6〜7は0x00
try { xRes = (int)(Enum.Parse(typeof(RegLength), Register)); } // レジスターのビット数を取得
catch { return false; }
xRes = (int)(Math.Ceiling(xRes / 8.0)); // xResに要求するレスポンスを計算
return IsOk = true; // 完成フラグを設定してリターン
}
switch (Command) // 回転方向が関わるコマンド DIR情報の設定は先頭バイトのBit0
{
case "Run":
case "StepClock":
case "Move":
case "GoTo_DIR":
case "GoUntil":
case "ReleseSW":
if (string.IsNullOrEmpty(Direction)) return false; // DirectionがSetされているか
switch (Direction.ToLower())
{
case "forward": Binary[0] |= 0x01; break; // 先頭バイト Bit0 ← 1 正回転
case "reverse": Binary[0] &= 0xFE; break; // 先頭バイト Bit0 ← 0 逆回転
default: return false;
}
break;
}
switch (Command) // アクションが関わるコマンド ACT情報の設定は先頭バイトのBit3
{
case "GoUntil":
case "ReleseSW":
if (string.IsNullOrEmpty(Action)) return false; // ActionがSetされているか
switch (Action.ToLower())
{
case "true": Binary[0] |= 0x08; break; // 先頭バイト Bit3 ← 1 有効
case "false": Binary[0] &= 0xF7; break; // 先頭バイト Bit3 ← 0 無効
default: return false;
}
break;
}
switch (Command) // 値をセットする必要があるコマンド
{
case "Run":
case "GoUntil": SetValue(0xFFFFF); break;
case "Move":
case "GoTo":
case "GoTo_DIR": SetValue(0x3FFFFF); break;
}
xRes = Command.Equals("GetStatus") ? 2 : 0;
return IsOk = true;
}
}//End of class CmdProperty
public class DaisyChain
{
public CmdProperty[] CmdSet { get; set; }
public byte[] SendData { get; private set; }
public int xSend { get; private set; }
public int xDevice { get; private set; }
public int xRecive { get; private set; }
public byte[,] ReciveData { get; private set; }
public string[,] strRecive { get; private set; }
public DaisyChain(int DeviceCount)
{
if (DeviceCount == 0) return;
CmdSet = new CmdProperty[DeviceCount];
for (xDevice = 0; xDevice < DeviceCount; xDevice++) CmdSet[xDevice] = new CmdProperty();
strRecive = new string[xDevice, 4];
}
public bool GetSendData()
{
int maxBin = 0, maxRes = 0;
for (int i = 0; i < xDevice; i++)
{
if (!CmdSet[i].IsOk) return false;
if (CmdSet[i].xBin > maxBin) maxBin = CmdSet[i].xBin;
if (CmdSet[i].xRes > maxRes) maxRes = CmdSet[i].xRes;
}
xRecive = maxRes * xDevice;
xSend = maxBin * xDevice + 2;
SendData = null;
SendData = new byte[xSend];
SendData[0] = (byte)(xSend - 1);
SendData[1] = (byte)(maxRes * xDevice);
int index = 2, P = 0, mark = 1;
do {
for (int DC = xDevice - 1; DC >= 0; DC--)
{
if (CmdSet[DC].xRes == 0) {
SendData[index++] = CmdSet[DC].xBin > P ? (byte)CmdSet[DC].Binary[P] : (byte)0;
} else {
SendData[index++] = mark == maxBin ? (byte)CmdSet[DC].Binary[0] : (byte)0;
}
}
P++;
mark++;
} while (index < xSend);
return true;
}
public bool FixReciveData(byte[] data)
{
if (data.Length != xRecive) return false;
int index = data.Length - 1;
int fixCnt = data.Length / xDevice;
ReciveData = null;
ReciveData = new byte[xDevice, fixCnt];
try
{
for (int DC = 0; DC < xDevice; DC++) {
index = xDevice - DC - 1;
int cnt = fixCnt - CmdSet[DC].xRes;
while (cnt < fixCnt) {
ReciveData[DC, cnt++] = data[index];
index += xDevice;
}
}
for (int DC = 0; DC < xDevice; DC++) {
index = fixCnt - 1;
for (int P = 3; P >= 0; P--) {
strRecive[DC, P] = (P + CmdSet[DC].xRes > 3) ? string.Format("{0:X2}", ReciveData[DC, index--]) : "";
}
}
return true;
} catch { return false; }
}
}//End of class DaisyChain
}
コメントをかく