public void SendData(List <double> values)
{
uint msgID = 0x80;
for (int i = 0; i < values.Count(); i++)
{
XLDefine.XL_Status txStatus;
XLClass.xl_event driverEvent = new XLClass.xl_event();
byte[] valueAsBytes = BitConverter.GetBytes(values[i]);
driverEvent.tagData.can_Msg.id = msgID;
driverEvent.tagData.can_Msg.dlc = 8;
driverEvent.tagData.can_Msg.data[0] = valueAsBytes[0];
driverEvent.tagData.can_Msg.data[1] = valueAsBytes[1];
driverEvent.tagData.can_Msg.data[2] = valueAsBytes[2];
driverEvent.tagData.can_Msg.data[3] = valueAsBytes[3];
driverEvent.tagData.can_Msg.data[4] = valueAsBytes[4];
driverEvent.tagData.can_Msg.data[5] = valueAsBytes[5];
driverEvent.tagData.can_Msg.data[6] = valueAsBytes[6];
driverEvent.tagData.can_Msg.data[7] = valueAsBytes[7];
driverEvent.tag = XLDefine.XL_EventTags.XL_TRANSMIT_MSG;
msgID++;
txStatus = driver.XL_CanTransmit(portHandle, txMask, driverEvent);
Console.WriteLine(txStatus);
}
}
void SendMflMessageExecute(object IdDlcStartBitLengthValueInterval)
{
string[] parameters = ((String)IdDlcStartBitLengthValueInterval).Split('_');
UInt16 id = Convert.ToUInt16(parameters[0]);
UInt16 dlc = Convert.ToUInt16(parameters[1]);
UInt16 length = Convert.ToUInt16(parameters[2]);
int startBit = Convert.ToUInt16(parameters[3]);
int value = Convert.ToInt32(parameters[4]);
int interval = Convert.ToInt32(parameters[5]);
int _iEventHandleLocal = -1;
int _iPortHandleLocal = -1;
UInt64 _ui64TransmitMaskLocal = 0;
xlStartCan(ref _iEventHandleLocal, ref _iPortHandleLocal, ref _ui64TransmitMaskLocal);
//XLClass.xl_can_message xlMsg = new XLClass.xl_can_message();
XLClass.xl_event_collection xlEventCollection = new XLClass.xl_event_collection(0);
XLClass.xl_event xlEvent;
xlEventCollection.xlEvent.Clear();
xlEvent = new XLClass.xl_event();
xlEvent.tagData.can_Msg.id = id;
xlEvent.tagData.can_Msg.dlc = dlc;
xlEvent.tagData.can_Msg.data = getMessageValue(new byte[dlc], value, startBit, length);
xlEvent.tag = (byte)XLClass.XLeventType.XL_TRANSMIT_MSG;
xlEventCollection.xlEvent.Add(xlEvent);
xlEventCollection.messageCount = (uint)xlEventCollection.xlEvent.Count;
for (int idx = 0; idx < interval; idx++)
{
_xlDriverTx.XL_CanTransmit(_iPortHandleLocal, /* _ui64TransmitMaskLocal*/ 1, xlEventCollection);
Thread.Sleep(1);
}
xlEventCollection.xlEvent.Clear();
xlEvent.tagData.can_Msg.data = xlEvent.tagData.can_Msg.data = getMessageValue(new byte[dlc], 0, startBit, length);
xlEventCollection.xlEvent.Add(xlEvent);
_xlDriverTx.XL_CanTransmit(_iPortHandleLocal, /* _ui64TransmitMaskLocal*/ 1, xlEventCollection);
_xlDriverTx.XL_ClosePort(_iPortHandleLocal);
_xlDriverTx.XL_CloseDriver();
}
public void CanTp_ReceptionInd()
{
// Create new object containing received data
XLClass.xl_event receivedEvent = new XLClass.xl_event();
// Result of XL Driver function calls
XLDefine.XL_Status xlStatus = XLDefine.XL_Status.XL_SUCCESS;
// Result values of WaitForSingleObject
XLDefine.WaitResults waitResult = new XLDefine.WaitResults();
byte diagFrameType;
byte[] diagPDU = new byte[1024];
int diagPDULength = 0, diagPDUoffset = 0;
byte diagSequenceNum = 0;
byte diagFlowState;
// Note: this thread will be destroyed by MAIN
while (true)
{
// Wait for hardware events
waitResult = (XLDefine.WaitResults)WaitForSingleObject(eventHandle, 1000);
if (waitFrameType == 0)
{
continue;
}
// If event occurred...
if (waitResult != XLDefine.WaitResults.WAIT_TIMEOUT)
{
// ...init xlStatus first
xlStatus = XLDefine.XL_Status.XL_SUCCESS;
// afterwards: while hw queue is not empty...
while (xlStatus != XLDefine.XL_Status.XL_ERR_QUEUE_IS_EMPTY)
{
// ...block RX thread to generate RX-Queue overflows
while (blockRxThread)
{
Thread.Sleep(1000);
}
// ...receive data from hardware.
xlStatus = CANTPDriver.XL_Receive(portHandle, ref receivedEvent);
// If receiving succeed....
if (xlStatus == XLDefine.XL_Status.XL_SUCCESS)
{
Console.WriteLine(receivedEvent.tagData.can_Msg.id);
if ((receivedEvent.flags & XLDefine.XL_MessageFlags.XL_EVENT_FLAG_OVERRUN) != 0)
{
Console.WriteLine("-- XL_EVENT_FLAG_OVERRUN --");
}
// ...and data is a Rx msg...
if (receivedEvent.tag == XLDefine.XL_EventTags.XL_RECEIVE_MSG)
{
if (receivedEvent.tagData.can_Msg.id == this.RxID)
{
Console.WriteLine(CANTPDriver.XL_GetEventString(receivedEvent));
diagFrameType = (byte)(receivedEvent.tagData.can_Msg.data[0] >> 4);
switch (diagFrameType)
{
//Single Frame
case 0:
if (waitFrameType == 1)
{
diagPDULength = receivedEvent.tagData.can_Msg.data[0] & 0x0F;
for (int i = 0; i < diagPDULength; i++)
{
diagPDU[i] = receivedEvent.tagData.can_Msg.data[i + 1];
}
OnDiagPduReceived(diagPDU, diagPDULength);
Console.WriteLine("PDU Length:" + diagPDULength);
}
break;
//FirstFrame
case 1:
if (waitFrameType == 1)
{
diagPDULength = (receivedEvent.tagData.can_Msg.data[0] & 0x0F) * 256 + (receivedEvent.tagData.can_Msg.data[1]);
for (int i = 0; i < 6; i++)
{
diagPDU[i] = receivedEvent.tagData.can_Msg.data[i + 2];
}
diagPDUoffset = 6;
diagSequenceNum = 0;
CanTpSendFlowControlFrame();
waitFrameType = 2;
Console.WriteLine("PDU Length:" + diagPDULength);
}
break;
//ConsecutiveFrame
case 2:
if (waitFrameType == 2)
{
if (diagSequenceNum + 1 == (byte)(receivedEvent.tagData.can_Msg.data[0] & 0x0F))
{
diagSequenceNum = (byte)(receivedEvent.tagData.can_Msg.data[0] & 0x0F);
for (int i = diagPDUoffset; i < diagPDUoffset + 7; i++)
{
diagPDU[i] = receivedEvent.tagData.can_Msg.data[i + 1];
}
if (diagPDUoffset == diagPDULength)
{
OnDiagPduReceived(diagPDU, diagPDULength);
}
}
}
break;
//FlowControl
case 3:
if (waitFrameType == 3)
{
diagFlowState = (byte)(receivedEvent.tagData.can_Msg.data[0] & 0x0F);
this.TxdiagBlockSize = receivedEvent.tagData.can_Msg.data[1];
this.TxdiagSTmin = receivedEvent.tagData.can_Msg.data[2];
waitforFlowControlEvent.Set();
}
break;
}
}
//if ((receivedEvent.tagData.can_Msg.flags & XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_OVERRUN) != 0)
//{
// Console.WriteLine("-- XL_CAN_MSG_FLAG_OVERRUN --");
//}
//// ...check various flags
//if ((receivedEvent.tagData.can_Msg.flags & XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_ERROR_FRAME)
// == XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_ERROR_FRAME)
//{
// Console.WriteLine("ERROR FRAME");
//}
//else if ((receivedEvent.tagData.can_Msg.flags & XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_REMOTE_FRAME)
// == XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_REMOTE_FRAME)
//{
// Console.WriteLine("REMOTE FRAME");
//}
//else
//{
// Console.WriteLine(CANTPDriver.XL_GetEventString(receivedEvent));
//}
}
}
}
}
// No event occurred
}
}
public void RXHANDLER()
{
//Boolean response = false;
// Create new object containing received data
XLClass.xl_event receivedEvent = new XLClass.xl_event();
// Result of XL Driver function calls
XLDefine.XL_Status xlStatus = XLDefine.XL_Status.XL_SUCCESS;
// Note: this thread will be destroyed by MAIN
while (true)
{
// Wait for hardware events
if (xlEvWaitHandle.WaitOne(5000))
{
// ...init xlStatus first
xlStatus = XLDefine.XL_Status.XL_SUCCESS;
// afterwards: while hw queue is not empty...
while (xlStatus != XLDefine.XL_Status.XL_ERR_QUEUE_IS_EMPTY)
{
// ...block RX thread to generate RX-Queue overflows
while (blockRxThread)
{
Thread.Sleep(1000);
}
// ...receive data from hardware.
xlStatus = UDSDemo.XL_Receive(portHandle, ref receivedEvent);
// If receiving succeed....
if (xlStatus == XLDefine.XL_Status.XL_SUCCESS)
{
if ((receivedEvent.flags & XLDefine.XL_MessageFlags.XL_EVENT_FLAG_OVERRUN) != 0)
{
}
// ...and data is a Rx msg...
if (receivedEvent.tag == XLDefine.XL_EventTags.XL_RECEIVE_MSG)
{
if ((receivedEvent.tagData.can_Msg.flags & XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_OVERRUN) != 0)
{
}
// ...check various flags
if ((receivedEvent.tagData.can_Msg.flags & XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_ERROR_FRAME)
== XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_ERROR_FRAME)
{
Console.WriteLine("Error frame" + Environment.NewLine);
}
else if ((receivedEvent.tagData.can_Msg.flags & XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_REMOTE_FRAME)
== XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_REMOTE_FRAME)
{
Console.WriteLine("Remote frame" + Environment.NewLine);
}
else if ((receivedEvent.tagData.can_Msg.id == RES_ID) && (receivedEvent.chanIndex == 2))
{
RTrigger = 1;
switch (receivedEvent.tagData.can_Msg.data[1])
{
case 0X7F: //for negative responses
Console.WriteLine("Negative Response Received!" + Environment.NewLine); // works fine
TRACE_DATA = string.Format(" {0:X2} {1} {2:X2} {3:X2} {4:X2} {5:X2} {6:X2} {7:X2} {8:X2} {9:X2} ", receivedEvent.tagData.can_Msg.id, receivedEvent.tagData.can_Msg.dlc,
receivedEvent.tagData.can_Msg.data[0], receivedEvent.tagData.can_Msg.data[1], receivedEvent.tagData.can_Msg.data[2], receivedEvent.tagData.can_Msg.data[3],
receivedEvent.tagData.can_Msg.data[4], receivedEvent.tagData.can_Msg.data[5], receivedEvent.tagData.can_Msg.data[6], receivedEvent.tagData.can_Msg.data[7]);
Console.ForegroundColor = ConsoleColor.DarkRed;
Console.WriteLine("RX: " + xlStatus + Environment.NewLine);
Console.WriteLine("RX Data: " + TRACE_DATA + Environment.NewLine);
Console.ForegroundColor = ConsoleColor.White;
response = false;
break;
default: //for all other responses
Console.WriteLine("Response Received, Positive Response" + Environment.NewLine); // works fine
TRACE_DATA = string.Format(" {0:X2} {1} {2:X2} {3:X2} {4:X2} {5:X2} {6:X2} {7:X2} {8:X2} {9:X2} ", receivedEvent.tagData.can_Msg.id, receivedEvent.tagData.can_Msg.dlc,
receivedEvent.tagData.can_Msg.data[0], receivedEvent.tagData.can_Msg.data[1], receivedEvent.tagData.can_Msg.data[2], receivedEvent.tagData.can_Msg.data[3],
receivedEvent.tagData.can_Msg.data[4], receivedEvent.tagData.can_Msg.data[5], receivedEvent.tagData.can_Msg.data[6], receivedEvent.tagData.can_Msg.data[7]);
Console.ForegroundColor = ConsoleColor.Green;
Console.WriteLine("RX: " + xlStatus + Environment.NewLine);
Console.WriteLine("RX Data: " + TRACE_DATA + Environment.NewLine);
Console.ForegroundColor = ConsoleColor.White;
response = true;
break;
}
}
}
}
}
}
}
}
public static void rx_thread2()
{
xlSingleChannelCAN_Port rxChannel = CAN_RxChannel2;
XLClass.xl_event receiveEvent = new XLClass.xl_event();
XLClass.XLstatus xlStatus = XLClass.XLstatus.XL_SUCCESS;
WaitResults waitResult = new WaitResults();
//CAN_RxChannel.xlCanAddAcceptanceRange(0x60, 0x90);
//CAN_RxChannel.xlCanAddAcceptanceRange(2, 3);
while (true)
{
waitResult = (WaitResults)WaitForSingleObject(rxChannel.eventHandle, 1000); // event handler
try
{
if (waitResult != WaitResults.WAIT_TIMEOUT)
{
xlStatus = XLClass.XLstatus.XL_SUCCESS;
while (xlStatus != XLClass.XLstatus.XL_ERR_QUEUE_IS_EMPTY)
{
xlStatus = rxChannel.xlReceive(ref receiveEvent);
if (xlStatus == XLClass.XLstatus.XL_SUCCESS)
{
byte[] data = receiveEvent.tagData.can_Msg.data;
uint msgID = receiveEvent.tagData.can_Msg.id;
uint msgDLC = receiveEvent.tagData.can_Msg.dlc;
//Console.WriteLine((msgID-0x10).ToString());
//Console.WriteLine(msgID.ToString());
if (msgID >= 0x10 & msgID < 0x61 )//& msgDLC == 6)
{
uint mNum = msgID - 0x10;
int[] motor = new int[boardMonotNum[0, mNum]];
byte[] tempDeg = new byte[4]; // temperary hold for degree
int mult = 1; // direction holder
double finalDeg = 0; // final degree
for (int i = 0; i < motor.Length; i++) // fill motor numbers
{
motor[i] = boardMonotNum[1, mNum] + i;
}
//if (mNum == 0x23)
// Console.WriteLine(mNum.ToString());
for (int i = 0; i < motor.Length; i++)
{
if (motor.Length == 2)
{
//Console.Write("2 : ");
tempDeg[0] = data[0 + i * 3];
tempDeg[1] = data[1 + i * 3];
tempDeg[2] = (byte)(data[2 + i * 3] & 0x7F);
tempDeg[3] = 0;
if (data[2 + i * 3] > 127)
{
mult = -1;
}
else
{
mult = 1;
}
int tempInt = BitConverter.ToInt32(tempDeg, 0);
tempInt = tempInt * mult;
//public static int[,] ratio = new int[5, 51]; // driven, drive, harmonic, enc, quad
finalDeg = getDeg(tempInt, ratio[0, motor[i]], ratio[1, motor[i]], ratio[2, motor[i]], ratio[3, motor[i]], ratio[4, motor[i]]);// (tempInt / (double)((ratio[1, motor[i]] / ratio[0, motor[i]]) * ratio[2, motor[i]] * ratio[3, motor[i]] * ratio[4, motor[i]])) * 360.0;
//finalDeg = (tempInt / (double)((ratio[1, motor[i]] / ratio[0, motor[i]]) * ratio[2, motor[i]] * ratio[3, motor[i]] * ratio[4, motor[i]])) * 360.0;
//sendUDP(motor[i], finalDeg);
}
else if (motor.Length == 1)
{
//Console.Write("1 : ");
tempDeg[0] = data[0 + i * 3];
tempDeg[1] = data[1 + i * 3];
tempDeg[2] = (byte)(data[2 + i * 3] & 0x7F);
tempDeg[3] = 0;
if (data[2 + i * 3] > 127)
{
mult = -1;
}
else
{
mult = 1;
}
int tempInt = BitConverter.ToInt32(tempDeg, 0);
tempInt = tempInt * mult;
//public static int[,] ratio = new int[5, 51]; // driven, drive, harmonic, enc, quad
//finalDeg = (tempInt / (double)((ratio[1, motor[i]] / ratio[0, motor[i]]) * ratio[2, motor[i]] * ratio[3, motor[i]] * ratio[4, motor[i]])) * 360.0;
finalDeg = getDeg(tempInt , ratio[0, motor[i]], ratio[1, motor[i]], ratio[2, motor[i]], ratio[3, motor[i]], ratio[4, motor[i]]);
// getDeg(int ticks, double drive , double driven , double harmonic, double enc, double quad)
/*
if (mNum == 0x23)
{
// dan edit
//Console.WriteLine(tempInt.ToString());
//finalDeg = 25;
//Console.WriteLine(finalDeg.ToString());
Console.WriteLine("mNum: " + mNum.ToString().ToString() + "motor: " + motor[i].ToString() + " @: " + ratio[3, motor[i] + 1].ToString());
}
* */
}
else if (motor.Length == 3)
{
//Console.Write("3 : ");
tempDeg[0] = data[0 + i * 2];
tempDeg[1] = (byte)(data[1 + i * 2] & 0x7F);
tempDeg[2] = 0;
tempDeg[3] = 0;
if (data[1 + i * 2] > 127)
{
mult = -1;
}
else
{
mult = 1;
}
int tempInt = BitConverter.ToInt32(tempDeg, 0);
tempInt = tempInt * mult;
if (mult == -1)
{
tempInt = -(32768 + tempInt);
}
/*
if (motor[i] == 1)
{
Console.WriteLine(tempInt.ToString());
}*/
//public static int[,] ratio = new int[5, 51]; // driven, drive, harmonic, enc, quad
finalDeg = getDeg(tempInt, ratio[0, motor[i]], ratio[1, motor[i]], ratio[2, motor[i]], ratio[3, motor[i]], ratio[4, motor[i]]);
//finalDeg = (tempInt / (double)((ratio[1, motor[i]] / ratio[0, motor[i]]) * ratio[2, motor[i]] * ratio[3, motor[i]] * ratio[4, motor[i]])) * 360.0;
//sendUDP(motor[i], finalDeg);
}
if (doDebug)
Console.Write("-");
sendUDP((byte)motor[i], finalDeg,0,0,0,0); // send the udp message
sendUDP((byte)motor[i], finalDeg, 0, 0, 0, 0); //dan
sendUDP((byte)motor[i], finalDeg, 0, 0, 0, 0); //dan
sendUDP((byte)motor[i], finalDeg, 0, 0, 0, 0); //dan
}
}
}
}
}
}
catch (Exception e)
{
}
}
}
public static void rx_thread()
{
xlSingleChannelCAN_Port rxChannel = CAN_RxChannel;
XLClass.xl_event receiveEvent = new XLClass.xl_event();
XLClass.XLstatus xlStatus = XLClass.XLstatus.XL_SUCCESS;
WaitResults waitResult = new WaitResults();
//CAN_RxChannel.xlCanAddAcceptanceRange(0x60, 0x90);
//CAN_RxChannel.xlCanAddAcceptanceRange(2, 3);
while (true)
{
waitResult = (WaitResults)WaitForSingleObject(rxChannel.eventHandle, 1000); // event handler
try
{
if (waitResult != WaitResults.WAIT_TIMEOUT)
{
xlStatus = XLClass.XLstatus.XL_SUCCESS;
while (xlStatus != XLClass.XLstatus.XL_ERR_QUEUE_IS_EMPTY)
{
xlStatus = rxChannel.xlReceive(ref receiveEvent);
if (xlStatus == XLClass.XLstatus.XL_SUCCESS)
{
byte[] data = receiveEvent.tagData.can_Msg.data;
uint msgID = receiveEvent.tagData.can_Msg.id;
uint msgDLC = receiveEvent.tagData.can_Msg.dlc;
//if ((msgID - 0x10)==28)
//Console.WriteLine((msgID).ToString());
//Console.WriteLine(msgID.ToString());
#region FT Sensor
if (msgID >= 0x40 & msgID < 0x50)// & msgDLC == 6)
{
uint nFT = msgID - 0x40; // FT number
// note: nFT = 1 --> right foot
// nFT = 2 --> left foot
// nFT = 6 --> right wrist
int Mx = BitConverter.ToInt16(data, 0);
int My = BitConverter.ToInt16(data, 2);
int Fz = BitConverter.ToInt16(data, 4);
sendUDP((byte)msgID, 0.0, Mx, My, Fz, 0); // send the udp message
sendUDP((byte)msgID, 0.0, Mx, My, Fz, 0); //dan
sendUDP((byte)msgID, 0.0, Mx, My, Fz, 0); //dan
sendUDP((byte)msgID, 0.0, Mx, My, Fz, 0); //dan
if (nFT == 1)
{
//uint MMx = BitConverter.ToUInt16(data, 0);
//Console.WriteLine("Mx = " + Mx.ToString());
}
}
if (msgID >= 0x50 & msgID < 0x60)
{
uint nAD = msgID - 0x50;
// note: nAD = 6 --> Works but do not know which ones it is yet
// nAD = 1 --> IMU (gyro = pitch and roll (1 and 2))
// nAD = 2 --> some other data prob IMU
// nAD = 5 --> better IMU
int Acc1 = BitConverter.ToInt16(data, 0);
int Acc2 = BitConverter.ToInt16(data, 2);
int Gyro1 = BitConverter.ToInt16(data, 4);
int Gyro2 = BitConverter.ToInt16(data, 6);
sendUDP((byte)msgID, 0.0, Acc1, Acc2, Gyro1, Gyro2);
sendUDP((byte)msgID, 0.0, Acc1, Acc2, Gyro1, Gyro2); //dan
sendUDP((byte)msgID, 0.0, Acc1, Acc2, Gyro1, Gyro2); //dan
sendUDP((byte)msgID, 0.0, Acc1, Acc2, Gyro1, Gyro2); //dan
if (nAD == 1)
{
//Console.WriteLine("Gyro2 = " + Gyro2.ToString());
}
}
#endregion
#region Motor Pos
if (msgID >= 0x10 & msgID < 0x40) // & msgDLC == 6)
{
uint mNum = msgID - 0x10;
int[] motor = new int[boardMonotNum[0, mNum]];
byte[] tempDeg = new byte[4]; // temperary hold for degree
int mult = 1; // direction holder
double finalDeg = 0; // final degree
for (int i = 0; i < motor.Length; i++) // fill motor numbers
{
motor[i] = boardMonotNum[1, mNum] + i;
}
for (int i = 0; i < motor.Length; i++)
{
if (motor.Length == 2)
{
//Console.Write("2 : ");
tempDeg[0] = data[0 + i * 3];
tempDeg[1] = data[1 + i * 3];
tempDeg[2] = (byte)(data[2 + i * 3] & 0x7F);
tempDeg[3] = 0;
if (data[2 + i * 3] > 127)
{
mult = -1;
}
else
{
mult = 1;
}
int tempInt = BitConverter.ToInt32(tempDeg, 0);
tempInt = tempInt * mult;
//public static int[,] ratio = new int[5, 51]; // driven, drive, harmonic, enc, quad
finalDeg = getDeg(tempInt, ratio[0, motor[i]], ratio[1, motor[i]], ratio[2, motor[i]], ratio[3, motor[i]], ratio[4, motor[i]]);// (tempInt / (double)((ratio[1, motor[i]] / ratio[0, motor[i]]) * ratio[2, motor[i]] * ratio[3, motor[i]] * ratio[4, motor[i]])) * 360.0;
//finalDeg = (tempInt / (double)((ratio[1, motor[i]] / ratio[0, motor[i]]) * ratio[2, motor[i]] * ratio[3, motor[i]] * ratio[4, motor[i]])) * 360.0;
//sendUDP(motor[i], finalDeg);
}
else if (motor.Length == 1)
{
//Console.Write("1 : ");
tempDeg[0] = data[0 + i * 3];
tempDeg[1] = data[1 + i * 3];
tempDeg[2] = (byte)(data[2 + i * 3] & 0x7F);
tempDeg[3] = 0;
if (data[2 + i * 3] > 127)
{
mult = -1;
}
else
{
mult = 1;
}
int tempInt = BitConverter.ToInt32(tempDeg, 0);
tempInt = tempInt * mult;
//public static int[,] ratio = new int[5, 51]; // driven, drive, harmonic, enc, quad
//finalDeg = (tempInt / (double)((ratio[1, motor[i]] / ratio[0, motor[i]]) * ratio[2, motor[i]] * ratio[3, motor[i]] * ratio[4, motor[i]])) * 360.0;
finalDeg = getDeg(tempInt, ratio[0, motor[i]], ratio[1, motor[i]], ratio[2, motor[i]], ratio[3, motor[i]], ratio[4, motor[i]]);
}
else if (motor.Length == 3)
{
//Console.Write("3 : ");
tempDeg[0] = data[0 + i * 2];
tempDeg[1] = (byte)(data[1 + i * 2] & 0x7F);
tempDeg[2] = 0;
tempDeg[3] = 0;
if (data[1 + i * 2] > 127)
{
mult = -1;
}
else
{
mult = 1;
}
int tempInt = BitConverter.ToInt32(tempDeg, 0);
tempInt = tempInt * mult;
if (mult == -1)
{
tempInt = -(32768 + tempInt);
}
//public static int[,] ratio = new int[5, 51]; // driven, drive, harmonic, enc, quad
finalDeg = getDeg(tempInt, ratio[0, motor[i]], ratio[1, motor[i]], ratio[2, motor[i]], ratio[3, motor[i]], ratio[4, motor[i]]);
//finalDeg = (tempInt / (double)((ratio[1, motor[i]] / ratio[0, motor[i]]) * ratio[2, motor[i]] * ratio[3, motor[i]] * ratio[4, motor[i]])) * 360.0;
//sendUDP(motor[i], finalDeg);
}
if (doDebug)
Console.Write("|");
sendUDP((byte)motor[i], finalDeg,0,0,0,0); // send the udp message
sendUDP((byte)motor[i], finalDeg, 0, 0, 0, 0); //dan
sendUDP((byte)motor[i], finalDeg, 0, 0, 0, 0); //dan
sendUDP((byte)motor[i], finalDeg, 0, 0, 0, 0); //dan
}
}
#endregion
}
}
}
}
catch (Exception e)
{
}
}
}
void ObserveExecute()
{
bwRx.WorkerReportsProgress = true;
bwRx.WorkerSupportsCancellation = true;
bwRx.DoWork += delegate(object s, DoWorkEventArgs args)
{
int time;
time = 0;
xlStartCan(ref _iEventHandleRx, ref _iPortHandleRx, ref _ui64TransmitMaskRx);
while (true)
{
if (bwRx.CancellationPending)
{
args.Cancel = true;
log("Observation stopped");
break;
}
else
{
XLClass.xl_event xlReceivedEvent = new XLClass.xl_event();
XLClass.XLstatus xlStatus = XLClass.XLstatus.XL_SUCCESS;
WaitResults lbWaitResult = new WaitResults();
lbWaitResult = (WaitResults)WaitForSingleObject(_iEventHandleRx, 1000);
if (lbWaitResult != WaitResults.WAIT_TIMEOUT)
{
xlStatus = XLClass.XLstatus.XL_SUCCESS;
while (xlStatus != XLClass.XLstatus.XL_ERR_QUEUE_IS_EMPTY)
{
xlStatus = _xlDriverRx.XL_Receive(_iPortHandleRx, ref xlReceivedEvent);
if (xlStatus == XLClass.XLstatus.XL_SUCCESS)
{
if ((xlReceivedEvent.flags & (byte)XLClass.XLeventFlags.XL_EVENT_FLAG_OVERRUN) != 0)
{
//throw new VectorCANcaseXLException("XL_EVENT_FLAG_OVERRUN");
}
if (xlReceivedEvent.tag == (byte)XLClass.XLeventType.XL_RECEIVE_MSG)
{
if ((xlReceivedEvent.tagData.can_Msg.flags & (ushort)XLClass.XLmessageFlags.XL_CAN_MSG_FLAG_OVERRUN) != 0)
{
//throw new VectorCANcaseXLException("XL_CAN_MSG_FLAG_OVERRUN");
}
if ((xlReceivedEvent.tagData.can_Msg.flags & (ushort)XLClass.XLmessageFlags.XL_CAN_MSG_FLAG_ERROR_FRAME) == (ushort)XLClass.XLmessageFlags.XL_CAN_MSG_FLAG_ERROR_FRAME)
{
//throw new VectorCANcaseXLException("ERROR FRAME");
}
else if ((xlReceivedEvent.tagData.can_Msg.flags & (ushort)XLClass.XLmessageFlags.XL_CAN_MSG_FLAG_REMOTE_FRAME) == (ushort)XLClass.XLmessageFlags.XL_CAN_MSG_FLAG_REMOTE_FRAME)
{
//throw new VectorCANcaseXLException("REMOTE FRAME");
}
else if (
((xlReceivedEvent.tagData.can_Msg.flags & (ushort)XLClass.XLmessageFlags.XL_CAN_MSG_FLAG_NERR) == 0) &&
((xlReceivedEvent.tagData.can_Msg.flags & (ushort)XLClass.XLmessageFlags.XL_CAN_MSG_FLAG_TX_COMPLETED) == 0) &&
((xlReceivedEvent.tagData.can_Msg.flags & (ushort)XLClass.XLmessageFlags.XL_CAN_MSG_FLAG_TX_REQUEST) == 0)
)
{
foreach (CMessageViewModel msg in SolutionCurrentProject.Messages)
{
if (msg.MessageId == xlReceivedEvent.tagData.can_Msg.id)
{
msg.MessageData = xlReceivedEvent.tagData.can_Msg.data;
}
}
}
}
}
}
}
bwRx.ReportProgress(1, ++time);
}
Thread.Sleep(1);
}
};
bwRx.ProgressChanged += delegate(object s, ProgressChangedEventArgs args)
{
SolutionCurrentObservationTime = (int)args.UserState;
//SolutionCurrentProject.RaisePropertyChanged("Messages");
};
bwRx.RunWorkerAsync();
}
/// <summary>
/// The trigger command for the sequence, which is processed asynchronous
/// </summary>
void StartExecute()
{
CurrentSendetMessages.Clear();
bwTx.WorkerReportsProgress = true;
bwTx.WorkerSupportsCancellation = true;
bwTx.DoWork += delegate(object s, DoWorkEventArgs args)
{
CSequenceViewModel sequence = (CSequenceViewModel)args.Argument;
Dictionary <int, CSentMessageViewModel> dicCurrentSendetMessages = new Dictionary <int, CSentMessageViewModel>();
XLClass.xl_can_message xlMsg = new XLClass.xl_can_message();
XLClass.xl_event_collection xlEventCollection = new XLClass.xl_event_collection(0);
XLClass.xl_event xlEvent;
int time;
time = 0;
xlStartCan(ref _iEventHandleTx, ref _iPortHandleTx, ref _ui64TransmitMaskTx);
while (true)
{
if (bwTx.CancellationPending)
{
_xlDriverTx.XL_ClosePort(_iPortHandleTx);
_xlDriverTx.XL_CloseDriver();
args.Cancel = true;
break;
}
else
{
foreach (CStepViewModel step in sequence.Steps)
{
if (time == step.StepTime)
{
switch (step.StepCommand)
{
case "Set":
{
if (!dicCurrentSendetMessages.ContainsKey(step.StepMessage.MessageId))
{
dicCurrentSendetMessages.Add(step.StepMessage.MessageId, new CSentMessageViewModel(step));
}
dicCurrentSendetMessages[step.StepMessage.MessageId].Message.MessageData = getMessageValue(step.StepMessage.MessageData, step.StepValue, step.StepSignal.SignalStartBit, step.StepSignal.SignalLenght);
dicCurrentSendetMessages[step.StepMessage.MessageId].StartTime = Convert.ToInt32(time);
dicCurrentSendetMessages[step.StepMessage.MessageId].StopTime = Int32.MaxValue;
break;
}
case "Set with ASR":
{
break;
}
case "Step 500ms":
{
if (!dicCurrentSendetMessages.ContainsKey(step.StepMessage.MessageId))
{
dicCurrentSendetMessages.Add(step.StepMessage.MessageId, new CSentMessageViewModel(step));
}
dicCurrentSendetMessages[step.StepMessage.MessageId].Message.MessageData = getMessageValue(step.StepMessage.MessageData, step.StepValue, step.StepSignal.SignalStartBit, step.StepSignal.SignalLenght);
dicCurrentSendetMessages[step.StepMessage.MessageId].StartTime = time;
dicCurrentSendetMessages[step.StepMessage.MessageId].StopTime = time + 500;
break;
}
case "Timeout":
{
if (dicCurrentSendetMessages.ContainsKey(step.StepMessage.MessageId))
{
dicCurrentSendetMessages.Remove(step.StepMessage.MessageId);
}
break;
}
case "Break":
{
if (MessageBox.Show(string.Format("Breaked sequence {0}, do you want to continue?", this.SolutionCurrentSequence.SequenceName), "Break", MessageBoxButton.YesNo, MessageBoxImage.Hand) != MessageBoxResult.Yes)
{
return;
}
break;
}
case "Restart":
{
//RestartExecute();
break;
}
default:
{
break;
}
}
}
}
foreach (KeyValuePair <int, CSentMessageViewModel> msg in dicCurrentSendetMessages)
{
if (msg.Value.StopTime == time)
{
msg.Value.Message.MessageData = getMessageValue(msg.Value.Message.MessageData, 0, msg.Value.Signal.SignalStartBit, msg.Value.Signal.SignalLenght);
}
}
xlEventCollection.xlEvent.Clear();
foreach (KeyValuePair <int, CSentMessageViewModel> msg in dicCurrentSendetMessages)
{
xlEvent = new XLClass.xl_event();
xlEvent.tagData.can_Msg.id = (uint)msg.Value.Message.MessageId;
xlEvent.tagData.can_Msg.dlc = (ushort)msg.Value.Message.MessageDlc;
xlEvent.tagData.can_Msg.data = msg.Value.Message.MessageData;
xlEvent.tag = (byte)XLClass.XLeventType.XL_TRANSMIT_MSG;
xlEventCollection.xlEvent.Add(xlEvent);
}
xlEventCollection.messageCount = (uint)xlEventCollection.xlEvent.Count;
if (xlEventCollection.messageCount > 0)
{
_xlDriverTx.XL_CanTransmit(_iPortHandleTx, _ui64TransmitMaskTx, xlEventCollection);
}
}
bwTx.ReportProgress(time++, dicCurrentSendetMessages);
Thread.Sleep(1);
}
};
bwTx.ProgressChanged += bwTx_ProgressChanged;
CurrentSendetMessages.Clear();
bwTx.RunWorkerAsync(this._currentSequenceViewModel);
}
public string Receive()
{
string dataFromDriver = "";
for (int i = 0; i < 8; i++)
{
ReceivedData[i] = 0;
}
XLClass.xl_event receivedEvent = new XLClass.xl_event();
XLDefine.XL_Status xlStatus = XLDefine.XL_Status.XL_SUCCESS;
XLDefine.WaitResults waitResult = (XLDefine.WaitResults)WaitForSingleObject(eventHandle, 1000);
if (waitResult != XLDefine.WaitResults.WAIT_TIMEOUT)
{
dataFromDriver += $"[{DateTime.Now.Hour:D2} : {DateTime.Now.Minute:D2} : {DateTime.Now.Second:D2} : {DateTime.Now.Millisecond:D3}] ";
while (xlStatus != XLDefine.XL_Status.XL_ERR_QUEUE_IS_EMPTY)
{
xlStatus = driver.XL_Receive(portHandle, ref receivedEvent);
switch (receivedEvent.tag)
{
case XLDefine.XL_EventTags.XL_LIN_MSG:
string dir = "RX".PadRight(3);
if ((receivedEvent.tagData.linMsgApi.linMsg.flags & XLDefine.XL_MessageFlags.XL_LIN_MSGFLAG_TX)
== XLDefine.XL_MessageFlags.XL_LIN_MSGFLAG_TX)
{
dir = "TX".PadRight(3);
}
else
{
for (int i = 0; i < 8; i++)
{
ReceivedData[i] = receivedEvent.tagData.linMsgApi.linMsg.data[i];
}
}
dataFromDriver += ("XL_LIN_MSG, " + dir + " id: " +
receivedEvent.tagData.linMsgApi.linMsg.id.ToString("X2").PadRight(3) + ", data: " +
receivedEvent.tagData.linMsgApi.linMsg.data[0].ToString("X2") + " " +
receivedEvent.tagData.linMsgApi.linMsg.data[1].ToString("X2") + " " +
receivedEvent.tagData.linMsgApi.linMsg.data[2].ToString("X2") + " " +
receivedEvent.tagData.linMsgApi.linMsg.data[3].ToString("X2") + " " +
receivedEvent.tagData.linMsgApi.linMsg.data[4].ToString("X2") + " " +
receivedEvent.tagData.linMsgApi.linMsg.data[5].ToString("X2") + " " +
receivedEvent.tagData.linMsgApi.linMsg.data[6].ToString("X2") + " " +
receivedEvent.tagData.linMsgApi.linMsg.data[7].ToString("X2")) + " (cha-1) " +
receivedEvent.chanIndex.ToString();
break;
case XLDefine.XL_EventTags.XL_LIN_ERRMSG:
dataFromDriver += ("XL_LIN_ERRMSG");
break;
case XLDefine.XL_EventTags.XL_LIN_SYNCERR:
dataFromDriver += ("XL_LIN_SYNCERR");
break;
case XLDefine.XL_EventTags.XL_LIN_NOANS:
dataFromDriver += ("XL_LIN_NOANS") + " (cha-1) " + receivedEvent.chanIndex.ToString();
break;
case XLDefine.XL_EventTags.XL_LIN_WAKEUP:
dataFromDriver += ("XL_LIN_WAKEUP");
break;
case XLDefine.XL_EventTags.XL_LIN_SLEEP:
dataFromDriver += ("XL_LIN_SLEEP");
break;
case XLDefine.XL_EventTags.XL_LIN_CRCINFO:
dataFromDriver += ("XL_LIN_CRCINFO");
break;
}
}
}
else
{
dataFromDriver += "Timeout";
}
//Trace.WriteLine(dataFromDriver);
driver.XL_FlushReceiveQueue(portHandle);
return(dataFromDriver);
}
public void SendMessage(int type)
{
XLDefine.XL_Status txStatus;
XLClass.xl_event driverEvent = new XLClass.xl_event();
if (type == FULL_THROTTLE_ON)
{
Console.WriteLine("Full Throttle On");
// FULL THROTTLE ON
driverEvent.tagData.can_Msg.id = 0x103;
driverEvent.tagData.can_Msg.dlc = 2;
driverEvent.tagData.can_Msg.data[0] = 19;
driverEvent.tagData.can_Msg.data[1] = 136;
driverEvent.tag = XLDefine.XL_EventTags.XL_TRANSMIT_MSG;
}
if (type == FULL_THROTTLE_OFF)
{
Console.WriteLine("Full Throttle Off");
// FULL THROTTLE OFF
driverEvent.tagData.can_Msg.id = 0x103;
driverEvent.tagData.can_Msg.dlc = 2;
driverEvent.tagData.can_Msg.data[0] = 0;
driverEvent.tagData.can_Msg.data[1] = 0;
driverEvent.tag = XLDefine.XL_EventTags.XL_TRANSMIT_MSG;
}
if (type == FULL_BRAKE_ON)
{
Console.WriteLine("Full Brake On");
// FULL BRAKE ON
driverEvent.tagData.can_Msg.id = 0x5;
driverEvent.tagData.can_Msg.dlc = 8;
driverEvent.tagData.can_Msg.data[0] = 0;
driverEvent.tagData.can_Msg.data[1] = 100;
driverEvent.tagData.can_Msg.data[2] = 0;
driverEvent.tagData.can_Msg.data[3] = 0;
driverEvent.tagData.can_Msg.data[4] = 0;
driverEvent.tagData.can_Msg.data[5] = 0;
driverEvent.tagData.can_Msg.data[6] = 0;
driverEvent.tagData.can_Msg.data[7] = 0;
driverEvent.tag = XLDefine.XL_EventTags.XL_TRANSMIT_MSG;
}
if (type == FULL_BRAKE_OFF)
{
Console.WriteLine("Full Brake Off");
// FULL BRAKE OFF
driverEvent.tagData.can_Msg.id = 0x100;
driverEvent.tagData.can_Msg.dlc = 8;
driverEvent.tagData.can_Msg.data[0] = 0;
driverEvent.tagData.can_Msg.data[1] = 0;
driverEvent.tagData.can_Msg.data[2] = 0;
driverEvent.tagData.can_Msg.data[3] = 0;
driverEvent.tagData.can_Msg.data[4] = 0;
driverEvent.tagData.can_Msg.data[5] = 0;
driverEvent.tagData.can_Msg.data[6] = 0;
driverEvent.tagData.can_Msg.data[7] = 0;
driverEvent.tag = XLDefine.XL_EventTags.XL_TRANSMIT_MSG;
}
// Transmit events
txStatus = driver.XL_CanTransmit(portHandle, txMask, driverEvent);
//Console.WriteLine(txStatus);
}
// -----------------------------------------------------------------------------------------------
/// <summary>
/// EVENT THREAD (RX)
///
/// RX thread waits for Vector interface events and displays filtered CAN messages.
/// </summary>
// -----------------------------------------------------------------------------------------------
public void RXThread()
{
// Create new object containing received data
XLClass.xl_event receivedEvent = new XLClass.xl_event();
// Result of XL Driver function calls
XLDefine.XL_Status xlStatus = XLDefine.XL_Status.XL_SUCCESS;
// Result values of WaitForSingleObject
XLDefine.WaitResults waitResult = new XLDefine.WaitResults();
// Note: this thread will be destroyed by MAIN
while (true)
{
// Wait for hardware events
//waitResult = (XLDefine.WaitResults)WaitForSingleObject(eventHandle, 1000);
// If event occurred...
if (waitResult != XLDefine.WaitResults.WAIT_TIMEOUT)
{
// ...init xlStatus first
xlStatus = XLDefine.XL_Status.XL_SUCCESS;
// afterwards: while hw queue is not empty...
while (xlStatus != XLDefine.XL_Status.XL_ERR_QUEUE_IS_EMPTY)
{
// ...block RX thread to generate RX-Queue overflows
while (blockRxThread)
{
Thread.Sleep(1000);
}
// ...receive data from hardware.
xlStatus = CANDemo.XL_Receive(portHandle, ref receivedEvent);
// If receiving succeed....
if (xlStatus == XLDefine.XL_Status.XL_SUCCESS)
{
if ((receivedEvent.flags & XLDefine.XL_MessageFlags.XL_EVENT_FLAG_OVERRUN) != 0)
{
Console.WriteLine("-- XL_EVENT_FLAG_OVERRUN --");
}
// ...and data is a Rx msg...
if (receivedEvent.tag == XLDefine.XL_EventTags.XL_RECEIVE_MSG)
{
if ((receivedEvent.tagData.can_Msg.flags & XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_OVERRUN) != 0)
{
Console.WriteLine("-- XL_CAN_MSG_FLAG_OVERRUN --");
}
// ...check various flags
if ((receivedEvent.tagData.can_Msg.flags & XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_ERROR_FRAME)
== XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_ERROR_FRAME)
{
Console.WriteLine("ERROR FRAME");
}
else if ((receivedEvent.tagData.can_Msg.flags & XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_REMOTE_FRAME)
== XLDefine.XL_MessageFlags.XL_CAN_MSG_FLAG_REMOTE_FRAME)
{
Console.WriteLine("REMOTE FRAME");
}
else
{
Console.WriteLine(CANDemo.XL_GetEventString(receivedEvent));
}
}
}
}
}
// No event occurred
}
}
