public static extern UInt32 VCI_Receive(UInt32 DeviceType, UInt32 DeviceInd, UInt32 CANInd, ref VCI_CAN_OBJ pReceive, UInt32 Len, Int32 WaitTime);
public static extern UInt32 VCI_Transmit(UInt32 DeviceType, UInt32 DeviceInd, UInt32 CANInd, ref VCI_CAN_OBJ pSend, UInt32 Len);
/// <summary>
/// 解析馬達回傳資料
/// </summary>
/// <param name="recobj">[IN] recobj 參數</param>
/// <param name="i">[IN] Index</param>
public static unsafe void AnalysisMotorData(VCI_CAN_OBJ recobj, uint i)
{
if (recobj.ID == 391) //ID187 轉向站號
{
byte Datalen = (byte)(m_recobj_2[i].DataLen % 9);
int LowByte = Convert.ToInt16(recobj.Data[4]);
int HiByte = Convert.ToInt16(recobj.Data[5]);
LowByte -= MainForm.L_Error;
HiByte -= MainForm.H_Error;
double LowDegree = (LowByte * 1.40625) / 255;
double HiDegree = HiByte * 1.40625;
double AngleTemp = LowDegree + HiDegree;
if (AngleTemp > 180) AngleTemp = AngleTemp - 360;
GlobalVar.RealMotorAngle = AngleTemp;
}
if (recobj.ID == 395) //ID187 行走站號
{
byte Datalen = (byte)(m_recobj_2[i].DataLen % 9);
int LowByte = Convert.ToInt16(recobj.Data[2]);
int HiByte = Convert.ToInt16(recobj.Data[3]);
if (HiByte > 128)
{
HiByte = 255 - HiByte;
LowByte = 255 - LowByte;
double Temp = -((double)HiByte * (double)256 + (double)LowByte) / 6.97265625;
if (Temp < -255) Temp = -255;
GlobalVar.RealMotorPower = Temp;
}
else
{
double Temp = ((double)HiByte * (double)256 + (double)LowByte) / 6.97265625;
if (Temp > 255) Temp = 255;
GlobalVar.RealMotorPower = Temp;
}
}
}
/// <summary>
/// 傳送資料給堆高機的馬達
/// </summary>
/// <param name="ID">[IN] 傳送ID</param>
/// <param name="len">[IN] 數據長度</param>
/// <param name="data">[IN] 傳送數據</param>
public static unsafe void SendCommandToMotor(uint ID, int len, byte[] data)
{
if (m_bOpen == 0) return;
VCI_CAN_OBJ sendobj = new VCI_CAN_OBJ();
sendobj.RemoteFlag = 0;
sendobj.ExternFlag = 0;
sendobj.ID = ID;
sendobj.DataLen = System.Convert.ToByte(len);
for (int i = 0; i < len; i++) sendobj.Data[i] = data[i];
CanBusTool.VCI_Transmit(m_devtype, m_devind_2, m_canind_2, ref sendobj, 1);
}
public static extern UInt32 VCI_Receive(UInt32 DeviceType, UInt32 DeviceInd, UInt32 CANInd, ref VCI_CAN_OBJ pReceive, UInt32 Len, Int32 WaitTime);
public static extern UInt32 VCI_Transmit(UInt32 DeviceType, UInt32 DeviceInd, UInt32 CANInd, ref VCI_CAN_OBJ pSend, UInt32 Len);
/// <summary>
/// 攔截堆高機電腦傳送過來資料，修改後傳送給馬達
/// </summary>
unsafe private static void StartComputerMonitorThread()
{
while (true)
{
UInt32 res = new UInt32();
res = CanBusTool.VCI_Receive(m_devtype, m_devind, m_canind, ref m_recobj[0], 1000, 100);
if (res > 10000 || res < 0)
{
return;
}
for (UInt32 i = 0; i < res; i++)
{
if (!CanControl) //初始化
{
//收到電腦傳送過來資料，直接傳送給馬達
VCI_CAN_OBJ recobj = m_recobj[i];
string StrID = Convert.ToString(recobj.ID, 16);
//Console.WriteLine("ReceiveComputerID: " + StrID);
byte Datalen = (byte)(m_recobj[i].DataLen % 9);
byte[] TransData = new byte[Datalen];
for (int k = 0; k < TransData.Length; k++)
{
TransData[k] = recobj.Data[k];
}
SendCommandToMotor(recobj.ID, Datalen, TransData);
//ID 207 轉向馬達
if (recobj.ID == 519)
{
Array.Copy(TransData, ReceiveComputer_Storage, TransData.Length);
Array.Copy(TransData, ReceiveComputer, TransData.Length);
}
//ID 20B 行走馬達
if (recobj.ID == 523)
{
Array.Copy(TransData, ReceiveMoveComputer_Storage, TransData.Length);
Array.Copy(TransData, ReceiveMoveComputer, TransData.Length);
}
}
else //開始介入
{
VCI_CAN_OBJ recobj = m_recobj[i];
string StrID = Convert.ToString(recobj.ID, 16);
byte Datalen = (byte)(recobj.DataLen % 9);
//收到ID 207
if (recobj.ID == 519) //轉向馬達站號
{
//將上次傳送結果複製給比較buffer
Array.Copy(ReceiveComputer, BufferData, BufferData.Length);
//更改ID207資料
if (isSend)
{
Array.Copy(TransData, ReceiveComputer, TransData.Length);//傳送資料
isSend = false;
}
//計算ID207的Check的資料
CalCheckData();
//傳送指令給馬達
SendCommandToMotor(recobj.ID, ReceiveComputer.Length, ReceiveComputer);
//回傳給電腦虛擬馬達ID187資料
SendCommand(391, ReceiveMotor_Storage.Length, ReceiveMotor_Storage);
}
if (recobj.ID == 523) //行走馬達站號
{
//修改ID523資料
if (isMoveSend)
{
Array.Copy(TransMoveData, ReceiveMoveComputer, TransMoveData.Length);//傳送資料
isMoveSend = false;
}
//傳送指令給馬達
SendCommandToMotor(recobj.ID, ReceiveMoveComputer.Length, ReceiveMoveComputer);
//回傳給電腦虛擬馬達ID18B資料
SendCommand(395, ReceiveMoveMotor_Storage.Length, ReceiveMoveMotor_Storage);
}
}
}
}
}
