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}; } // コンストラクタ:バイナリをとりあえず1バイトだけ確保
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); // レジスターのビット数で許容できる範囲をマスクして2バイト以降に設定する
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
}