/// <summary>
/// Applies the directives of an Action to this cursor.
/// </summary>
/// <remarks>
/// While this Dictionary dispatch pattern is a bit convoluted, it is faster than dynamic casting,
/// more stable and allows for compiler-time checks and non-error fallback.
/// https://chodounsky.net/2014/01/29/dynamic-dispatch-in-c-number/
/// </remarks>
/// <param name="action"></param>
/// <returns></returns>
public bool Apply(Action action)
{
Type t = action.GetType();
if (ActionsMap.ContainsKey(t))
{
return(ActionsMap[t](action, this));
}
logger.Verbose($"Found no suitable method for Action \"{action}\"");
return(false);
}
/// <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");
}
}
public bool ConnectToDevice(string ip, int port)
{
if (connectionMode == ConnectionType.Machina)
{
logger.Error("Try ConnectToDevice() instead");
return(false);
}
// Sanity
if (!_driver.ConnectToDevice(ip, port))
{
logger.Error("Cannot connect to device");
return(false);
}
else
{
InitializeRobotCursors();
}
logger.Info("Connected to " + parentRobot.Brand + " robot \"" + parentRobot.Name + "\" on " + _driver.IP + ":" + _driver.Port);
logger.Verbose("TCP:");
logger.Verbose(" " + this.IssueCursor.position.ToString(true));
logger.Verbose(" " + new Orientation(this.IssueCursor.rotation).ToString(true));
logger.Verbose(" " + this.IssueCursor.axes.ToString(true));
if (this.IssueCursor.externalAxesCartesian != null)
{
logger.Verbose("External Axes (TCP):");
logger.Verbose(" " + this.IssueCursor.externalAxesCartesian.ToString(true));
}
if (this.IssueCursor.externalAxesJoints != null)
{
logger.Verbose("External Axes (J): ");
logger.Verbose(" " + this.IssueCursor.externalAxesJoints.ToString(true));
}
return(true);
}