/// <summary>
/// 读Can2数据
/// </summary>
/// <param name="msgRead"></param>
/// <param name="nReadCount"></param>
/// <param name="pulNumberofRead"></param>
/// <param name="error"></param>
/// <returns></returns>
public bool ReadCan2RecogInfor(out AdvCan.canmsg_t[] msgRead, uint nReadCount, out uint pulNumberofRead, out string error)
{
error = string.Empty;
pulNumberofRead = 0;
msgRead = new AdvCan.canmsg_t[1];
msgRead[0].data = new byte[8];
int nRet;
bool success2 = true;
nRet = Defs.Device2.acCanRead(msgRead, nReadCount, ref pulNumberofRead);
if (nRet == AdvCANIO.TIME_OUT)
{
error = "receiving timeout!";
success2 = false;
}
else if (nRet == AdvCANIO.OPERATION_ERROR)
{
error = " receiving error!";
success2 = false;
}
else
{
if (msgRead[0].id == AdvCan.ERRORID)
{
error = "is a incorrect package";
success2 = false;
}
}
return(success2);
}
/// <summary>
/// 傳送資料給堆高機的馬達
/// </summary>
/// <param name="ID">[IN] 傳送ID</param>
/// <param name="len">[IN] 數據長度</param>
/// <param name="data">[IN] 傳送數據</param>
unsafe public static void SendCommandToMotor(uint ID, int len, byte[] data, int k)
{
if (m_bOpen == 0)
{
return;
}
AdvCan.canmsg_t[] msgWrite = new AdvCan.canmsg_t[nMsgCount];
msgWrite[k].flags = 0;
msgWrite[k].id = ID;
msgWrite[k].length = System.Convert.ToByte(len);
msgWrite[k].data = new byte[AdvCan.DATALENGTH];
for (int i = 0; i < len; i++)
{
msgWrite[k].data[i] = data[i];
}
int nRet = Device_CommandToMotor.acCanWrite(msgWrite, nMsgCount, ref pulNumberofWritten_Device_MotorToCommand); //Send frames
/*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++) msgWrite[k].Data[i] = data[i];
//CanBusTool.VCI_Transmit(m_devtype, m_devind_2, m_canind_2, ref sendobj, 1);
}
unsafe public static void AnalysisMotorData(AdvCan.canmsg_t msgWrite, int i)
{
if (msgWrite.id == 391) //ID187 轉向站號
{
byte Datalen = (byte)(m_recobj_2[i].DataLen % 9);
int LowByte = Convert.ToInt16(msgWrite.data[4]);
int HiByte = Convert.ToInt16(msgWrite.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 (msgWrite.id == 395) //ID187 行走站號
{
byte Datalen = (byte)(m_recobj_2[i].DataLen % 9);
int LowByte = Convert.ToInt16(msgWrite.data[2]);
int HiByte = Convert.ToInt16(msgWrite.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>
/// 获取can1信息
/// </summary>
/// <returns></returns>
public bool GetDataFromCardcan(out string speed, out string current, out string sInfo)
{
sInfo = string.Empty;
speed = "0.00";
current = "0.00";
bool results = false;
#region 数据请求
AdvCan.canmsg_t[] msgWrite = new AdvCan.canmsg_t[2];
uint pulNumberofWritten;
msgWrite[0].flags = AdvCan.MSG_EXT;
msgWrite[0].cob = 0;
string idz = "19" + "0" + axis_IDnumber + "F0" + "00";
msgWrite[0].id = Convert.ToUInt32(idz, 16);
msgWrite[0].length = 8;
msgWrite[0].data = new byte[8];
msgWrite[0].data[0] = Convert.ToByte("00100000", 2);
msgWrite[0].data[1] = Convert.ToByte("01000011", 2);
msgWrite[0].data[2] = Convert.ToByte("00011111", 2);
msgWrite[0].data[3] = Convert.ToByte("F0", 16);
msgWrite[0].data[4] = Convert.ToByte("0" + axis_IDnumber, 16);
msgWrite[0].data[5] = Convert.ToByte("FF", 16);
msgWrite[0].data[6] = Convert.ToByte("FF", 16);
msgWrite[0].data[7] = Convert.ToByte("00", 16);
msgWrite[0].data[7] = computeSNP(msgWrite[0].data);
msgWrite[1].flags = AdvCan.MSG_EXT;
msgWrite[1].cob = 0;
string idw = "19" + "0" + axis_IDnumber + "F0" + "FF";
msgWrite[1].id = Convert.ToUInt32(idw, 16);
msgWrite[1].length = 8;
msgWrite[1].data = new byte[8];
msgWrite[1].data[0] = Convert.ToByte("3C", 16);
msgWrite[1].data[1] = Convert.ToByte("AA", 16);
msgWrite[1].data[2] = Convert.ToByte("01", 16);
msgWrite[1].data[3] = Convert.ToByte("01", 16);
msgWrite[1].data[4] = Convert.ToByte("55", 16);
msgWrite[1].data[5] = Convert.ToByte("55", 16);
msgWrite[1].data[6] = Convert.ToByte("55", 16);
msgWrite[1].data[7] = Convert.ToByte("55", 16);
bool WritE1;
bool WritE2;
lock (Defs._object1)
{
WritE1 = WriteCan1RecogInfor(msgWrite, 2, out pulNumberofWritten, out sInfo);
}
lock (Defs._object2)
{
WritE2 = WriteCan2RecogInfor(msgWrite, 2, out pulNumberofWritten, out sInfo);
}
if (WritE1 == true || WritE2 == true)
{
Defs.rwl.AcquireReaderLock(10);
try
{
speed = Defs.speed[3];
current = Defs.current[3];
}
finally
{
Defs.rwl.ReleaseReaderLock();
}
results = true;
}
#endregion
return(results);
}
/// <summary>
/// 力矩模式
/// </summary>
/// <param name="direction"></param>
/// <param name="rotorspeed"></param>
/// <param name="error"></param>
/// <returns></returns>
public bool WriteCardTorqueControl(string direction, string torque, out string error)
{
bool results = true;
error = string.Empty;
byte[] torques = new byte[3];
torques = TransTentoHexTorque(direction, torque);
AdvCan.canmsg_t[] msgWrite = new AdvCan.canmsg_t[2];
uint pulNumberofWritten;
msgWrite[0].flags = AdvCan.MSG_EXT;
msgWrite[0].cob = 0;
string idz = "19" + "0" + axis_IDnumber + "F0" + "00";
msgWrite[0].id = Convert.ToUInt32(idz, 16);
msgWrite[0].length = 8;
msgWrite[0].data = new byte[8];
msgWrite[0].data[0] = Convert.ToByte("20", 16);
msgWrite[0].data[1] = Convert.ToByte("33", 16);
msgWrite[0].data[2] = Convert.ToByte("1F", 16);
msgWrite[0].data[3] = Convert.ToByte("F0", 16);
msgWrite[0].data[4] = Convert.ToByte("0" + axis_IDnumber, 16);
msgWrite[0].data[5] = Convert.ToByte("FF", 16);
msgWrite[0].data[6] = Convert.ToByte("FF", 16);
msgWrite[0].data[7] = Convert.ToByte("00", 16);
msgWrite[0].data[7] = computeSNP(msgWrite[0].data);
msgWrite[1].flags = AdvCan.MSG_EXT;
msgWrite[1].cob = 0;
string idw = "19" + "0" + axis_IDnumber + "F0" + "FF";
msgWrite[1].id = Convert.ToUInt32(idw, 16);
msgWrite[1].length = 5;
msgWrite[1].data = new byte[8];
msgWrite[1].data[0] = Convert.ToByte("A5", 16);
msgWrite[1].data[1] = Convert.ToByte("1C", 16);
msgWrite[1].data[2] = torques[0];
msgWrite[1].data[3] = torques[1];
msgWrite[1].data[4] = torques[2];
bool WritE1;
bool WritE2;
lock (Defs._object1)
{
WritE1 = WriteCan1RecogInfor(msgWrite, 2, out pulNumberofWritten, out error);
}
lock (Defs._object2)
{
WritE2 = WriteCan2RecogInfor(msgWrite, 2, out pulNumberofWritten, out error);
}
if (WritE1 == false && WritE2 == false)
{
error = "力矩写错误";
return(false);
}
return(results);
}
/// <summary>
///
/// </summary>
unsafe public static void StartMotorMonitorThread_Adv()
{
AdvCan.canmsg_t[] msgWrite = new AdvCan.canmsg_t[nMsgCount]; //Package for write
//AdvCan.canmsg_t[] tmp_msgWrite = new AdvCan.canmsg_t[nMsgCount];
AdvCan.canmsg_t[] msgRead = new AdvCan.canmsg_t[nMsgCount];
uint nWriteCount = 0;
//uint pulNumberofWritten = 0;
//uint SendIndex = 0;
//string strTemp;
char[] data = new char[8];
//int i;
//Initialize msg
for (int j = 0; j < nMsgCount; j++)
{
msgWrite[j].flags = AdvCan.MSG_EXT;
msgWrite[j].cob = 0;
msgWrite[j].id = 0;
msgWrite[j].length = (short)AdvCan.DATALENGTH;
msgWrite[j].data = new byte[AdvCan.DATALENGTH];
if (rtr)
{
msgWrite[j].flags += AdvCan.MSG_RTR;
msgWrite[j].length = 0;
}
}
//string MotorMonitor_Adv = "";
uint nReadCount = nMsgCount;
uint pulNumberofRead = 0;
//uint ReceiveIndex = 0;
//myDelegate SetList = new myDelegate(ListDelegate);
//myDelegate SetButton = new myDelegate(ButtonDelegate);
//ReceiveIndex = 0;
m_bRun = true;
while (m_bRun)
{
int nRet;
//string StrID = "";
//MotorMonitor_Adv = "";
nRet = Device_MotorToCommand.acCanRead(msgRead, nReadCount, ref pulNumberofRead); //Receiving frames
if (nRet == AdvCANIO.TIME_OUT)
{
//labelCANStatus.
//ShowStatus.Invoke(SetList, ReceiveStatus);//Package receiving timeout
}
else if (nRet == AdvCANIO.OPERATION_ERROR)
{
//ShowStatus.Invoke(SetList, ReceiveStatus);
}
else
{
for (int j = 0; j < pulNumberofRead; j++)
{
if (msgRead[j].id == AdvCan.ERRORID)
{
//ReceiveStatus += "a incorrect package";
//ShowStatus.Invoke(SetList, ReceiveStatus);
}
else
{
if ((msgRead[j].flags & AdvCan.MSG_RTR) > 0)
{
//ReceiveStatus += "a RTR package";
}
else
{
for (int k = 0; k < msgRead[j].length; k++)
{
if (!CanControl) //初始化
{
string StrID = Convert.ToString(msgRead[j].id, 16);
data = StrID.ToCharArray();
for (int i = 0; i < data.Length; i++)
{
msgWrite[k].data[i] = Convert.ToByte(data[i] - 48);
}
msgWrite[k].length = (short)data.Length;
nWriteCount = nMsgCount;
SendMotorToCommand(msgWrite[k].id, msgWrite[k].length, msgWrite[k].data, k);
if (msgRead[j].id == 319)
{
Array.Copy(msgWrite[k].data, ReceiveComputer_Storage, data.Length);
Array.Copy(msgWrite[k].data, ReceiveComputer, data.Length);
}
//ID 20B 行走馬達
if (msgRead[j].id == 395)
{
Array.Copy(msgWrite[k].data, ReceiveMoveComputer_Storage, data.Length);
Array.Copy(msgWrite[k].data, ReceiveMoveComputer, data.Length);
}
}
else //開始介入
{
if (GlobalVar.isCanBusDebug) //記錄回傳角度資訊
{
//for (UInt32 i = 0; i < res; i++)
{
;
//分析馬達回傳資料
AnalysisMotorData(msgRead[j], j);
}
}
}
}
}
}
}
}
}
}
unsafe public static void StartComputerMonitorThread_Adv()
{
AdvCan.canmsg_t[] msgWrite = new AdvCan.canmsg_t[nMsgCount]; //Package for write
//AdvCan.canmsg_t[] tmp_msgWrite = new AdvCan.canmsg_t[nMsgCount];
AdvCan.canmsg_t[] msgRead = new AdvCan.canmsg_t[nMsgCount];
uint nWriteCount = 0;
//uint pulNumberofWritten = 0;
//uint SendIndex = 0;
//string strTemp;
byte[] data = new byte[8];
//int i;
//Initialize msg
for (int j = 0; j < nMsgCount; j++)
{
msgWrite[j].flags = AdvCan.MSG_EXT;
msgWrite[j].cob = 0;
msgWrite[j].id = 0;
msgWrite[j].length = (short)AdvCan.DATALENGTH;
msgWrite[j].data = new byte[AdvCan.DATALENGTH];
if (rtr)
{
msgWrite[j].flags += AdvCan.MSG_RTR;
msgWrite[j].length = 0;
}
}
//string MotorMonitor_Adv = "";
uint nReadCount = nMsgCount;
uint pulNumberofRead = 0;
//uint ReceiveIndex = 0;
//myDelegate SetList = new myDelegate(ListDelegate);
//myDelegate SetButton = new myDelegate(ButtonDelegate);
//ReceiveIndex = 0;
m_bRun = true;
while (m_bRun)
{
int nRet;
//string StrID = "";
//MotorMonitor_Adv = "";
nRet = Device_CommandToMotor.acCanRead(msgRead, nReadCount, ref pulNumberofRead); //Receiving frames
if (nRet == AdvCANIO.TIME_OUT)
{
//labelCANStatus.
//ShowStatus.Invoke(SetList, ReceiveStatus);//Package receiving timeout
}
else if (nRet == AdvCANIO.OPERATION_ERROR)
{
//ShowStatus.Invoke(SetList, ReceiveStatus);
}
else
{
for (int j = 0; j < pulNumberofRead; j++)
{
if (msgRead[j].id == AdvCan.ERRORID)
{
//ReceiveStatus += "a incorrect package";
//ShowStatus.Invoke(SetList, ReceiveStatus);
}
else
{
if ((msgRead[j].flags & AdvCan.MSG_RTR) > 0)
{
//ReceiveStatus += "a RTR package";
}
else
{
//for (int k = 0; k < msgRead[j].length; k++)
{
if (!CanControl) //初始化
{
string StrID = Convert.ToString(msgRead[j].id, 16);
data = msgRead[j].data;
for (int i = 0; i < data.Length; i++)
{
msgWrite[j].data[i] = Convert.ToByte(data[i]);
}
msgWrite[j].length = (short)data.Length;
nWriteCount = nMsgCount;
//Array.Copy(msgWrite, 0, tmp_msgWrite, 0, nMsgCount);
//nRet = Device_MotorToCommand.acCanWrite(tmp_msgWrite, nWriteCount, ref pulNumberofWritten); //Send frames
SendCommandToMotor(msgWrite[j].id, msgWrite[j].length, msgWrite[j].data, j);
if (msgRead[j].id == 519)
{
Array.Copy(msgWrite[j].data, ReceiveComputer_Storage, data.Length);
Array.Copy(msgWrite[j].data, ReceiveComputer, data.Length);
}
//ID 20B 行走馬達
if (msgRead[j].id == 523)
{
Array.Copy(msgWrite[j].data, ReceiveMoveComputer_Storage, data.Length);
Array.Copy(msgWrite[j].data, ReceiveMoveComputer, data.Length);
}
}
else //開始介入
{
//VCI_CAN_OBJ recobj = msgRead[j];
//string StrID = Convert.ToString(msgRead[j].id, 16);
/*for (int i = 0; i < data.Length; i++)
* {
* msgWrite[k].data[i] = Convert.ToByte(data[i] - 48);
* }
* msgWrite[k].length = (short)data.Length;
* nWriteCount = nMsgCount;
* Array.Copy(msgWrite, 0, tmp_msgWrite, 0, nMsgCount);*/
//收到ID 207
if (msgRead[j].id == 519) //轉向馬達站號
{
//將上次傳送結果複製給比較buffer
Array.Copy(ReceiveComputer, BufferData, BufferData.Length);
//更改ID207資料
if (isSend)
{
Array.Copy(TransData, ReceiveComputer, TransData.Length);//傳送資料
isSend = false;
}
//計算ID207的Check的資料
CalCheckData();
//傳送指令給馬達
SendCommandToMotor(msgRead[j].id, ReceiveComputer.Length, ReceiveComputer, j);
//nRet = Device_MotorToCommand.acCanWrite(tmp_msgWrite, nWriteCount, ref pulNumberofWritten); //Send frames
//回傳給電腦虛擬馬達ID187資料
SendMotorToCommand(391, ReceiveMotor_Storage.Length, ReceiveMotor_Storage, j);
//nRet = Device_CommandToMotor.acCanWrite(tmp_msgWrite, nWriteCount, ref pulNumberofWritten); //Send frames
}
if (msgRead[j].id == 523) //行走馬達站號
{
//修改ID523資料
if (isMoveSend)
{
Array.Copy(TransMoveData, ReceiveMoveComputer, TransMoveData.Length);//傳送資料
isMoveSend = false;
}
//傳送指令給馬達
SendCommandToMotor(msgRead[j].id, ReceiveMoveComputer.Length, ReceiveMoveComputer, j);
//回傳給電腦虛擬馬達ID18B資料
SendMotorToCommand(395, ReceiveMoveMotor_Storage.Length, ReceiveMoveMotor_Storage, j);
}
}
}
}
}
}
}
}
///舊的
/*while (m_bRun)
* {
* 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);
* }
* }
* }
* }*/
///舊的
////可用AGV
/*int nRet;
* AdvCan.canmsg_t[] msgWrite = new AdvCan.canmsg_t[nMsgCount]; //Package for write
* AdvCan.canmsg_t[] tmp_msgWrite = new AdvCan.canmsg_t[nMsgCount];
* uint nWriteCount = 0;
* uint pulNumberofWritten = 0;
* uint SendIndex = 0;
*
* string strTemp;
* char[] data = new char[8];
* int i;
*
* //Initialize msg
* for (int j = 0; j < nMsgCount; j++)
* {
* msgWrite[j].flags = AdvCan.MSG_EXT;
* msgWrite[j].cob = 0;
* msgWrite[j].id = 0;
* msgWrite[j].length = (short)AdvCan.DATALENGTH;
* msgWrite[j].data = new byte();
* if (rtr)
* {
* msgWrite[j].flags += AdvCan.MSG_RTR;
* msgWrite[j].length = 0;
* }
* }
*
* m_bRun = true;
* while (m_bRun)
* {
* if (count == true)
* {
* if (nWriteCount > 0)
* {
* Array.Copy(msgWrite, nWriteCount, tmp_msgWrite, 0, nMsgCount - nWriteCount);
* }
* else
* {
* for (int j = 0; j < nMsgCount; j++)
* {
* strTemp = Convert.ToString(SendIndex + 1 + j);
* data = strTemp.ToCharArray();
* for (i = 0; i < data.Length; i++)
* {
* msgWrite[j].data = Convert.ToByte(data[i] - 48);
* }
* msgWrite[j].length = (short)data.Length;
* nWriteCount = nMsgCount;
* Array.Copy(msgWrite, 0, tmp_msgWrite, 0, nMsgCount);
* }
* }
* count = false;
* }
*
*
* nRet = Device_CommandToMotor.acCanWrite(tmp_msgWrite, nWriteCount, ref pulNumberofWritten); //Send frames
* if (nRet == AdvCANIO.TIME_OUT)
* {
*
* }
* else if (nRet == AdvCANIO.OPERATION_ERROR)
* {
*
* }
* else
* {
* nWriteCount -= pulNumberofWritten;
* SendIndex += pulNumberofWritten;
* }
*
* Thread.Sleep(400);
* }*/
}
