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