private void ReadData(object sender, System.EventArgs e)
{
try
{
Station station = Project.ActiveProject as Station;
RsToolData tool = station.ActiveTask.ActiveTool;
Vector4 trans = tool.Frame.GlobalMatrix.t;
// x,y,z
trans.x *= 1000.0;
trans.y *= 1000.0;
trans.z *= 1000.0;
//rx ry rz
double rx = Globals.RadToDeg(tool.Frame.GlobalMatrix.EulerXYZ.x);
double ry = Globals.RadToDeg(tool.Frame.GlobalMatrix.EulerXYZ.y);
double rz = Globals.RadToDeg(tool.Frame.GlobalMatrix.EulerXYZ.z);
this.textBox_x.Text = trans.x.ToString();
this.textBox_y.Text = trans.y.ToString();
this.textBox_z.Text = trans.z.ToString();
this.textBox_RX.Text = rx.ToString();
this.textBox_RY.Text = ry.ToString();
this.textBox_RZ.Text = rz.ToString();
Mechanism mech = station.ActiveTask.Mechanism;
double[] jointValues = mech.GetJointValues();
this.textBox1.Text = Globals.RadToDeg(jointValues[0]).ToString();
this.textBox2.Text = Globals.RadToDeg(jointValues[1]).ToString();
this.textBox3.Text = Globals.RadToDeg(jointValues[2]).ToString();
this.textBox4.Text = Globals.RadToDeg(jointValues[3]).ToString();
this.textBox5.Text = Globals.RadToDeg(jointValues[4]).ToString();
this.textBox6.Text = Globals.RadToDeg(jointValues[5]).ToString();
this.textBox7.Text = Class1.energy_list[Class1.energy_list.Count - 1];
}
catch (Exception)
{
timer1.Enabled = true;
timer1.Interval = 500;
}
}
// Listener
static void MyJointListener(object sender, EventArgs e)
{
//Getting jointposition
Station station = Project.ActiveProject as Station;
Mechanism mech = station.ActiveTask.Mechanism;
double[] jointValues = mech.GetJointValues();
//Getting tool coordinates
RsToolData tool = station.ActiveTask.ActiveTool;
Vector4 trans = tool.Frame.GlobalMatrix.t;
Matrix4 transmat = tool.Frame.GlobalMatrix;
Vector3 transpos = new Vector3(trans.x, trans.y, trans.z);
// Only recording values if simulation is running
if (Simulator.State == SimulationState.Running)
{
JointValuesList.Add(jointValues);
ToolValuesList.Add(trans);
transposList.Add(transpos);
}
}
/// <summary>
/// Update Mechanism axes values
/// </summary>
/// <param name="component">Component that owns signals. </param>
/// <param name="jtValue">JointTarget Value</param>
private void UpdateAxis(SmartComponent component, Controller controller)
{
int iCtrlMechanismIndex = (int)component.Properties["CtrlMechanism"].Value;
if ((iCtrlMechanismIndex < 1) || (iCtrlMechanismIndex > controller.MotionSystem.MechanicalUnits.Count))
{
component.Properties["CtrlMechanism"].Value = iCtrlMechanismIndex = 1;
}
//Get Controller Values
ABB.Robotics.Controllers.MotionDomain.MechanicalUnit mu = controller.MotionSystem.MechanicalUnits[iCtrlMechanismIndex - 1];
int iCtrlNumberOfAxes = mu.NumberOfAxes;
JointTarget jtValue = mu.GetPosition();
component.Properties["CtrlSpecAxis"].Attributes["MaxValue"] = iCtrlNumberOfAxes.ToString();
int iCtrlSpecAxis = (int)component.Properties["CtrlSpecAxis"].Value;
if (iCtrlSpecAxis > iCtrlNumberOfAxes)
{
component.Properties["CtrlSpecAxis"].Value = iCtrlSpecAxis = iCtrlNumberOfAxes;
}
if (iCtrlSpecAxis < 0)
{
component.Properties["CtrlSpecAxis"].Value = iCtrlSpecAxis = 0;
}
//Get Mechanism Values
Mechanism mecha = (Mechanism)component.Properties["Mechanism"].Value;
if (mecha == null)
{
return;
}
int[] mechaAxes = mecha.GetActiveJoints();
int iMechNumberOfAxes = mechaAxes.Length;
double[] mechaValues = mecha.GetJointValues();
component.Properties["MechSpecAxis"].Attributes["MaxValue"] = iMechNumberOfAxes.ToString();
int iMechSpecAxis = (int)component.Properties["MechSpecAxis"].Value;
if (iMechSpecAxis > iCtrlNumberOfAxes)
{
component.Properties["MechSpecAxis"].Value = iMechSpecAxis = iMechNumberOfAxes;
}
if (iMechSpecAxis < 0)
{
component.Properties["MechSpecAxis"].Value = iMechSpecAxis = 0;
}
//Start Updating
int iAxesUpdated = 0;
if (iCtrlSpecAxis == 0)
{
if (iMechSpecAxis == 0)
{ //Take all Controller axes to update all Mechanism
for (int iAxis = 1; (iAxis <= iCtrlNumberOfAxes) && (iAxis <= iMechNumberOfAxes); ++iAxis)
{
mechaValues[iAxis - 1] = GetJointTargetAxis(jtValue, iAxis, mu.Type);
}
}
else
{ //Only Take Mechanism Specific axis from controller to update it
if ((iMechSpecAxis <= iCtrlNumberOfAxes) && (iMechSpecAxis <= iMechNumberOfAxes))
{
mechaValues[iMechSpecAxis - 1] = GetJointTargetAxis(jtValue, iMechSpecAxis, mu.Type);
}
}
}
else
{
if (iMechSpecAxis == 0)
{ //Only Take Controller Specific axis to update same in Mechanism
if ((iCtrlSpecAxis <= iCtrlNumberOfAxes) && (iCtrlSpecAxis <= iMechNumberOfAxes))
{
mechaValues[iCtrlSpecAxis - 1] = GetJointTargetAxis(jtValue, iCtrlSpecAxis, mu.Type);
}
}
else
{ //Only Take Controller Specific axis to update Mechanism Specific axis
if ((iCtrlSpecAxis <= iCtrlNumberOfAxes) && (iMechSpecAxis <= iMechNumberOfAxes))
{
mechaValues[iMechSpecAxis - 1] = GetJointTargetAxis(jtValue, iCtrlSpecAxis, mu.Type);
}
}
}
iAxesUpdated = ((iCtrlSpecAxis == 0) && (iMechSpecAxis == 0)) ? Math.Min(iCtrlNumberOfAxes, iMechNumberOfAxes) : 1;
//Updating
if (!mecha.SetJointValues(mechaValues, true))
{
component.Properties["Status"].Value = "Error";
Logger.AddMessage("RSMoveMecha: Component " + component.Name + " can't update " + mecha.Name + ".", LogMessageSeverity.Error);
}
else
{
string sNumberOfAxesStatus = iAxesUpdated.ToString() + "/" + iMechNumberOfAxes.ToString() + " Mechanism axes updated from " + iCtrlNumberOfAxes.ToString() + " Controller axes.";
component.Properties["NumberOfAxesStatus"].Value = sNumberOfAxesStatus;
}
}
