public override void _Process(float delta)
{
if (worker != null)
{
controller.tcp = worker.robot.currentTool;
controller.origin = worker.robot.currentOrigin;
if (worker.robot.currentCommand != null && !controller.finishedWithError)
{
controller.Move(worker.robot.currentCommand);
worker.robot.SetCurrentCommand(null);
shallNotify = true;
}
else if (!controller.isMoving && shallNotify)
{
shallNotify = false;
worker.robot.doneCommand = true;
worker.robot.currentPosition = controller.CurrentPosition(controller.q);
}
if (worker.error != null)
{
_errorLabel.Text = worker.error;
}
else if (controller.currentError != null)
{
_errorLabel.Text = controller.currentError;
}
else
{
_errorLabel.Text = "";
}
if (worker.doneEverything || controller.finishedWithError)
{
Abort();
}
}
Controller.Position currentPosition = controller.CurrentPosition(controller.q);
Vector3 cba = Controller.Quat2Abc(currentPosition.rot);
Vector3 xyz = currentPosition.pos;
_positionLabel.Text =
"{X = " + Round2(xyz.x) +
", Y = " + Round2(xyz.y) +
", Z = " + Round2(xyz.z) +
", A = " + Round2(Mathf.Rad2Deg(cba.z)) +
", B = " + Round2(Mathf.Rad2Deg(cba.y)) +
", C = " + Round2(Mathf.Rad2Deg(cba.x)) + "}";
}
public void origin(LuaTable table)
{
currentOrigin = LuaTable2Position(table);
System.Threading.Thread.Sleep(1);
}
public void tool(LuaTable table)
{
currentTool = LuaTable2Position(table);
System.Threading.Thread.Sleep(1);
}
