//Method that sets robots position
public static void SetRobPos(int setVal)
{
Station station2 = Project.ActiveProject as Station;
Mechanism mech = station2.ActiveTask.Mechanism;
mech.SetJointValues(RecData.JointValuesList[setVal], false);
//AddMarkup();
}
/// <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;
}
}
