//// @TODO: this will need to get reallocated when fixing stream mode...
//public StreamQueue streamQueue;
//██████╗ ██╗ ██╗██████╗ ██╗ ██╗ ██████╗
//██╔══██╗██║ ██║██╔══██╗██║ ██║██╔════╝
//██████╔╝██║ ██║██████╔╝██║ ██║██║
//██╔═══╝ ██║ ██║██╔══██╗██║ ██║██║
//██║ ╚██████╔╝██████╔╝███████╗██║╚██████╗
//╚═╝ ╚═════╝ ╚═════╝ ╚══════╝╚═╝ ╚═════╝
/// <summary>
/// Main constructor.
/// </summary>
public Control(Robot parentBot)
{
parentRobot = parentBot;
logger = parentRobot.logger;
// Reset();
_executionCursor = new RobotCursor(this, "ExecutionCursor", false, null);
_releaseCursor = new RobotCursor(this, "ReleaseCursor", false, _executionCursor);
_issueCursor = new RobotCursor(this, "IssueCursor", true, _releaseCursor);
SetControlMode(DEFAULT_CONTROLMODE);
SetConnectionMode(DEFAULT_CONNECTIONMODE);
}
/// <summary>
/// Modify a cursor's TCP transform according to a tool. Useful for Attach operations.
/// </summary>
/// <param name="tool"></param>
internal void ApplyToolTransformToCursor(RobotCursor cursor, Tool tool, RobotLogger logger, bool log)
{
// Now transform the cursor position to the tool's transformation params:
Vector worldVector = Vector.Rotation(tool.TCPPosition, cursor.rotation);
Vector newPos = cursor.position + worldVector;
Rotation newRot = Rotation.Combine(cursor.rotation, tool.TCPOrientation); // postmultiplication
cursor.prevPosition = cursor.position;
cursor.position = newPos;
cursor.prevRotation = cursor.rotation;
cursor.rotation = newRot;
//cursor.prevAxes = cursor.axes; // why was this here? joints don't change on tool attachment...
if (log)
{
logger.Verbose("Cursor TCP changed to " + cursor.position + " " + new Orientation(cursor.rotation) + " due to tool attachment");
}
}
/// <summary>
/// Undo tool-based TCP transformations on a cursor. Useful for Detach operations.
/// </summary>
/// <param name="tool"></param>
internal void UndoToolTransformOnCursor(RobotCursor cursor, Tool tool, RobotLogger logger, bool log)
{
// TODO: at some point in the future, check for translationFirst here
Rotation newRot = Rotation.Combine(cursor.rotation, Rotation.Inverse(tool.TCPOrientation)); // postmultiplication by the inverse rotation
Vector worldVector = Vector.Rotation(tool.TCPPosition, cursor.rotation);
Vector newPos = cursor.position - worldVector;
cursor.prevPosition = cursor.position;
cursor.position = newPos;
cursor.prevRotation = cursor.rotation;
cursor.rotation = newRot;
//this.prevAxes = this.axes;
//this.axes = null; // axes were null anyway...?
if (log)
{
logger.Verbose("Cursor TCP changed to " + cursor.position + " " + new Orientation(cursor.rotation) + " due to tool removal");
}
}
// ╔╗ ╔═╗╔═╗╔═╗
// ╠╩╗╠═╣╚═╗║╣
// ╚═╝╩ ╩╚═╝╚═╝
/// <summary>
/// Main constructor.
/// </summary>
/// <param name="name"></param>
/// <param name="applyImmediately"></param>
public RobotCursor(Control parentControl, string name, bool applyImmediately, RobotCursor childCursor)
{
this.parentControl = parentControl;
this.logger = parentControl.logger;
this.name = name;
this.applyImmediately = applyImmediately;
this.child = childCursor;
// @TODO: make this programmatic
if (this.parentControl.parentRobot.Brand == RobotType.HUMAN)
{
compiler = new CompilerHuman();
}
else if (this.parentControl.parentRobot.Brand == RobotType.MACHINA)
{
compiler = new CompilerMACHINA();
}
else if (this.parentControl.parentRobot.Brand == RobotType.ABB)
{
compiler = new CompilerABB();
}
else if (this.parentControl.parentRobot.Brand == RobotType.UR)
{
compiler = new CompilerUR();
}
else if (this.parentControl.parentRobot.Brand == RobotType.KUKA)
{
compiler = new CompilerKUKA();
}
else if (this.parentControl.parentRobot.Brand == RobotType.ZMORPH)
{
compiler = new CompilerZMORPH();
}
// Initialize buffers
actionBuffer = new ActionBuffer(this);
settingsBuffer = new SettingsBuffer();
}
