private void refreshRobotDataInstance()
{
if (aRTDE.isSynchronizing)
{
RTDEOutputObj OutObj = aRTDE.getOutputObj();
dataGridData otherData = RobotOtherData.Find(x => x.varName == "A1");
otherData.varValue = Math.Round(OutObj.actual_q.X / Math.PI * 180, 2);
otherData = RobotOtherData.Find(x => x.varName == "A2");
otherData.varValue = Math.Round(OutObj.actual_q.Y / Math.PI * 180, 2);
otherData = RobotOtherData.Find(x => x.varName == "A3");
otherData.varValue = Math.Round(OutObj.actual_q.Z / Math.PI * 180, 2);
otherData = RobotOtherData.Find(x => x.varName == "A4");
otherData.varValue = Math.Round(OutObj.actual_q.Rx / Math.PI * 180, 2);
otherData = RobotOtherData.Find(x => x.varName == "A5");
otherData.varValue = Math.Round(OutObj.actual_q.Ry / Math.PI * 180, 2);
otherData = RobotOtherData.Find(x => x.varName == "A6");
otherData.varValue = Math.Round(OutObj.actual_q.Rz / Math.PI * 180, 2);
}
}
private void timer1_Tick(object sender, EventArgs e)
{
if (aRTClient.Status == RTClientStatus.Syncing)
{
RTClientObj aObj = aRTClient.getRTClientObj();
textBox5.Text = aObj.actual_TCP_pose.ToString();
}
if (aRTDE.isSynchronizing)
{
RTDEOutputObj OutObj = aRTDE.getOutputObj();
textBox7.Text = "Actual tcp pose: " + OutObj.actual_TCP_pose.ToString() + "\n";
textBox7.Text = textBox7.Text + " Elbow position: " + OutObj.elbow_position.ToString();
}
}
private void refreshRobotDataInstance()
{
if (aRTDE.isSynchronizing)
{
RTDEOutputObj OutObj = aRTDE.getOutputObj();
//textBox7.Text = "Actual tcp pose: "+OutObj.actual_TCP_pose.ToString()+"\n";
//textBox7.Text =textBox7.Text+" Elbow position: "+ OutObj.elbow_position.ToString();
Console.WriteLine(OutObj.actual_q.ToString());
MainWindow.myRobotData.a1Value = Math.Round(OutObj.actual_q.X / Math.PI * 180, 2);
MainWindow.myRobotData.a2Value = Math.Round(OutObj.actual_q.Y / Math.PI * 180, 2);
MainWindow.myRobotData.a3Value = Math.Round(OutObj.actual_q.Z / Math.PI * 180, 2);
MainWindow.myRobotData.a4Value = Math.Round(OutObj.actual_q.Rx / Math.PI * 180, 2);
MainWindow.myRobotData.a5Value = Math.Round(OutObj.actual_q.Ry / Math.PI * 180, 2);
MainWindow.myRobotData.a6Value = Math.Round(OutObj.actual_q.Rz / Math.PI * 180, 2);
MainWindow.myRobotData.raw1Value = OutObj.actual_q.X;
MainWindow.myRobotData.raw2Value = OutObj.actual_q.Y;
MainWindow.myRobotData.raw3Value = OutObj.actual_q.Z;
MainWindow.myRobotData.raw4Value = OutObj.actual_q.Rx;
MainWindow.myRobotData.raw5Value = OutObj.actual_q.Ry;
MainWindow.myRobotData.raw6Value = OutObj.actual_q.Rz;
}
}
private IEnumerator timer2_Tick()
{
while (true)
{
try
{
aRTDE.startSync(); //启动 RTDE server 同步
}
catch
{
throw;
}
if (aRTDE.isSynchronizing)
{
RTDEOutputObj outobj = aRTDE.getOutputObj(); //读取 RTDE output 数据
CurTCPPos = outobj.actual_TCP_pose;
//CurPX = outobj.actual_TCP_pose.X;
//CurPY = outobj.actual_TCP_pose.Y;
//CurPZ = outobj.actual_TCP_pose.Z;
//CurRX = outobj.actual_TCP_pose.Rx;
//CurRY = outobj.actual_TCP_pose.Ry;
//CurRZ = outobj.actual_TCP_pose.Rz;
CurJointPos = outobj.actual_q;
//CurJ1 = outobj.actual_q.X;//机座
//CurJ2 = outobj.actual_q.Y;//肩部
//CurJ3 = outobj.actual_q.Z;//肘部
//CurJ4 = outobj.actual_q.Rx;//手腕1
//CurJ5 = outobj.actual_q.Ry;//手腕2
//CurJ6 = outobj.actual_q.Rz;//手腕3
string tcppose = outobj.actual_TCP_pose.ToString();
RTDEMessage aMsg = new RTDEMessage("Content of message", "Message source",
RTDEMessage.INFO_MESSAGE); //创建 RTDEMessage 对象， Warning Level 为 Info
aRTDE.sendMessage(aMsg); //发送 RTDEMessage
RTDEInputObj inobj = new RTDEInputObj(); //创建 RTDE input 对象
// 将数字输出置为 0b00000011,RTDEInputObj 同步数据一定要是 RTDEConfig.xml 文件中已经配置好的 input
inobj.standard_digital_output = 3;
aRTDE.setInputObj(inobj); //发送 input
}
//TCP当前位置
if (CurTCPPos != null)
{
//ActTCPTB_X.Text = (CurTCPPos.X * 1000).ToString("0.00");
//ActTCPTB_Y.Text = (CurTCPPos.Y * 1000).ToString("0.00");
//ActTCPTB_Z.Text = (CurTCPPos.Z * 1000).ToString("0.00");
//ActTCPTB_RX.Text = (CurTCPPos.Rx).ToString("0.00");
//ActTCPTB_RY.Text = (CurTCPPos.Ry).ToString("0.00");
//ActTCPTB_RZ.Text = (CurTCPPos.Rz).ToString("0.00");
}
//关节当前位置
if (CurJointPos != null)
{
Pos[0].text = (CurJointPos.X * 180 / PI).ToString("0.00");
Pos[1].text = (CurJointPos.Y * 180 / PI).ToString("0.00");
Pos[2].text = (CurJointPos.Z * 180 / PI).ToString("0.00");
Pos[3].text = (CurJointPos.Rx * 180 / PI).ToString("0.00");
Pos[4].text = (CurJointPos.Ry * 180 / PI).ToString("0.00");
Pos[5].text = (CurJointPos.Rz * 180 / PI).ToString("0.00");
}
aRTDE.stopSync(); //关闭 RTDE server 同步，释放资源
yield return(new WaitForSeconds(1f));
}
}
