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)
{
}
}
}
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)
{
}
}
}
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();
}
