private void getAccData_Click(object sender, EventArgs e) { if (string.Compare(com_port, "OFF") != 0 && Motion.getQ.Enabled == true) { double tolerance = 2.25; float x, y, z; arduino.pin_capture(8); DateTime time_start = DateTime.Now; while (!arduino.dataRecieved && (DateTime.Now - time_start).TotalMilliseconds < 100) { ; } arduino.dataRecieved = false; x = arduino.captured_float; arduino.pin_capture(9); time_start = DateTime.Now; while (!arduino.dataRecieved && (DateTime.Now - time_start).TotalMilliseconds < 100) { ; } arduino.dataRecieved = false; y = arduino.captured_float; arduino.pin_capture(10); time_start = DateTime.Now; while (!arduino.dataRecieved && (DateTime.Now - time_start).TotalMilliseconds < 100) { ; } arduino.dataRecieved = false; z = arduino.captured_float; accLXText.Text = (x - tolerance).ToString(); accHXText.Text = (x + tolerance).ToString(); accLYText.Text = (y - tolerance).ToString(); accHYText.Text = (y + tolerance).ToString(); accLZText.Text = (z - tolerance).ToString(); accHZText.Text = (z + tolerance).ToString(); } else { MessageBox.Show(cmd_lang_dic["errorMsg20"]); } }
private void update_autoframe(int i) { if (fcheck[i].Checked == true) { if ((fbox2[i].SelectedIndex != 0 || fbox3[i].SelectedIndex != 0) && getQ.Enabled == true) { try { if (renew_quaternion) { arduino.getQ(); DateTime time_start = DateTime.Now; while (!arduino.dataRecieved && (DateTime.Now - time_start).TotalMilliseconds < 100) { ; } arduino.dataRecieved = false; autoq.w = arduino.quaternion[0]; autoq.x = arduino.quaternion[1]; autoq.y = arduino.quaternion[2]; autoq.z = arduino.quaternion[3]; arduino.pin_capture(11); while (!arduino.dataRecieved && (DateTime.Now - time_start).TotalMilliseconds < 100) { ; } arduino.dataRecieved = false; omega[0] = 0.1 * omega[0] + 0.9 * arduino.captured_float; arduino.pin_capture(12); while (!arduino.dataRecieved && (DateTime.Now - time_start).TotalMilliseconds < 100) { ; } arduino.dataRecieved = false; omega[1] = 0.1 * omega[1] + 0.9 * arduino.captured_float; renew_quaternion = false; } RollPitchYaw rpy = (autoq.Normalized().Round(4) * q.Normalized().Round(4).Inverse()).toRPY(); int gain = 0; for (int src = 0; src < 2; src++) { if (src == 0 && p_gain[i] != 0) { if (fbox2[i].SelectedIndex == 1 || fbox2[i].SelectedIndex == 2) { gain += (int)Math.Round(rpy.rpy[fbox2[i].SelectedIndex - 1] * (180 / Math.PI) * p_gain[i]); } else if (fbox2[i].SelectedIndex == 3 || fbox2[i].SelectedIndex == 4) { gain += (int)Math.Round(omega[fbox2[i].SelectedIndex - 3] * p_gain[i]); } } else if (src == 1 && s_gain[i] != 0) { if (fbox3[i].SelectedIndex == 1 || fbox3[i].SelectedIndex == 2) { gain += (int)Math.Round(rpy.rpy[fbox3[i].SelectedIndex - 1] * (180 / Math.PI) * s_gain[i]); } else if (fbox3[i].SelectedIndex == 3 || fbox3[i].SelectedIndex == 4) { gain += (int)Math.Round(omega[fbox3[i].SelectedIndex - 3] * s_gain[i]); } } } autogain[i] = gain; } catch { autogain[i] = 0; } } else { autogain[i] = 0; } int pos = (int)homeframe[i] + offset[i] + autogain[i]; if ((uint)pos >= min[i] && (uint)pos <= Max[i]) { if (autoframe[i] != pos) { autoframe[i] = pos; send_msg = true; } return; } } autoframe[i] = 0; }