private void tbAdd_6axit_data(double AxisX, sIMU6 sensing) { DataRow rowMeas; rowMeas = tbMeas.NewRow(); rowMeas["time(ms)"] = AxisX; rowMeas["AccX"] = sensing.AccX; rowMeas["AccY"] = sensing.AccY; rowMeas["AccZ"] = sensing.AccZ; rowMeas["GyroX"] = sensing.Roll; rowMeas["GyroY"] = sensing.Pitch; rowMeas["GyroZ"] = sensing.Yaw; tbMeas.Rows.Add(rowMeas); }
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; } } }