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 } }