//----------------------------------------------------------------------------------------- public UDP_Data ReceiveUdpPacket() { UDP_Data udpData = new UDP_Data(); udpData.buffer = _listener.Receive(ref remoteClient); udpData.remoteClient = remoteClient; return(udpData); }
private sIMU6 ParseIMU6_fromUDP(UDP_Data inData) { sIMU6 sensing_data; int offset = 0; sensing_data.AccX = BitConverter.ToSingle(inData.payload, offset); offset += 4; sensing_data.AccY = BitConverter.ToSingle(inData.payload, offset); offset += 4; sensing_data.AccZ = BitConverter.ToSingle(inData.payload, offset); offset += 4; sensing_data.Roll = BitConverter.ToSingle(inData.payload, offset); offset += 4; sensing_data.Pitch = BitConverter.ToSingle(inData.payload, offset); offset += 4; sensing_data.Yaw = BitConverter.ToSingle(inData.payload, offset); offset += 4; return(sensing_data); }
//----------------------------------------------------------------------------------------- private void GetUdpPackets() { UDP_Data udp = _listener.ReceiveUdpPacket(); byte[] data = udp.buffer; bool dealtWith = false; if (data != null) { Packet packet = _serializer.Deserialize(data); dealtWith = CheckForNewUser(udp.remoteClient, packet.sender); if (dealtWith == false) { dealtWith = CheckForDisconnect(packet); } if (dealtWith == false) { _packetList.Add(packet); } } }
private void tmUDPListener_Tick(object sender, EventArgs e) { if (CurrentMode == 1) { if (dataLogSts) { grAcc.addPoint(ref chrtAcceleration, timePassed, new double[] { getRandomValue(), 1.5 + getRandomValue(), 2.5 + getRandomValue(), -1.0 + getRandomValue(), -1.5 + getRandomValue(), -2.5 + getRandomValue() }); grAcc.adjust_Yaxis_scale(ref chrtAcceleration, new double[] { getRandomValue(), 1.5 + getRandomValue(), 2.5 + getRandomValue(), -1.0 + getRandomValue(), -1.5 + getRandomValue(), -2.5 + getRandomValue() }); grGyro.addPoint(ref chrtGyroscope, timePassed, new double[] { getRandomValue(), getRandomValue(), getRandomValue() }); grGyro.adjust_Yaxis_scale(ref chrtGyroscope, new double[] { getRandomValue(), getRandomValue(), getRandomValue() }); } timePassed = timePassed + 100; return; } UDP_Data rcvPkt = udpHandle.getPacket(); if (rcvPkt.pkt_cmd != 0) { switch (rcvPkt.pkt_cmd) { case cUDP_CMD.uCMD_IMU_GET_VALUE: sIMU6 sensing_data = ParseIMU6_fromUDP(rcvPkt); tbAdd_6axit_data(timePassed, sensing_data); cBotPos pos = new cBotPos(); deg_normal = pos.deg_DegTheta(sensing_data.AccY, sensing_data.AccZ); txtTheta.Text = deg_normal.ToString(); if (dataLogSts) { grAcc.addPoint(ref chrtAcceleration, timePassed, new double[] { sensing_data.AccX, sensing_data.AccY, sensing_data.AccZ, sensing_data.Roll, sensing_data.Pitch, sensing_data.Yaw }); grAcc.adjust_Yaxis_scale(ref chrtAcceleration, new double[] { sensing_data.AccX, sensing_data.AccY, sensing_data.AccZ, sensing_data.Roll, sensing_data.Pitch, sensing_data.Yaw }); } timePassed = timePassed + 100; break; case cUDP_CMD.uCMD_PID_GET_PARAM: int offset = 0; double dVal = 0.0; cPID PIDctl = new cPID(); dVal = BitConverter.ToDouble(rcvPkt.payload, offset); offset += sizeof(double); PIDctl.curr_err = dVal; dVal = BitConverter.ToDouble(rcvPkt.payload, offset); offset += sizeof(double); PIDctl.accum_err = dVal; dVal = BitConverter.ToDouble(rcvPkt.payload, offset); offset += sizeof(double); PIDctl.P_reg = dVal; dVal = BitConverter.ToDouble(rcvPkt.payload, offset); offset += sizeof(double); PIDctl.I_reg = dVal; dVal = BitConverter.ToDouble(rcvPkt.payload, offset); offset += sizeof(double); PIDctl.D_reg = dVal; grGyro.addPoint(ref chrtGyroscope, timePassed, new double[] { PIDctl.curr_err, PIDctl.accum_err, PIDctl.curr_err, PIDctl.accum_err }); grGyro.adjust_Yaxis_scale(ref chrtGyroscope, new double[] { PIDctl.curr_err, PIDctl.accum_err, PIDctl.curr_err, PIDctl.accum_err }); txtPID_err.Text = string.Format("{0:0.0000}", PIDctl.curr_err); txtPID_accumErr.Text = string.Format("{0:0.0000}", PIDctl.accum_err); txtPID_Preg.Text = string.Format("{0:0.0000}", PIDctl.P_reg); txtPID_Ireg.Text = string.Format("{0:0.0000}", PIDctl.I_reg); txtPID_Dreg.Text = string.Format("{0:0.0000}", PIDctl.D_reg); txtPID_val.Text = string.Format("{0:0.0000}", PIDctl.P_reg + PIDctl.D_reg + PIDctl.D_reg); break; } } }