///// <summary>
///// Resets all internal state properties to default values. To be invoked upon
///// an internal robot reset.
///// @TODO rethink this
///// </summary>
//public void Reset()
//{
// virtualCursor = new RobotCursor(this, "virtualCursor", true);
// writeCursor = new RobotCursor(this, "writeCursor", false);
// virtualCursor.SetChild(writeCursor);
// motionCursor = new RobotCursor(this, "motionCursor", false);
// writeCursor.SetChild(motionCursor);
// areCursorsInitialized = false;
// SetControlMode(DEFAULT_CONTROLMODE);
// //currentSettings = new Settings(DEFAULT_SPEED, DEFAULT_ZONE, DEFAULT_MOTION_TYPE, DEFAULT_REFCS);
// //settingsBuffer = new SettingsBuffer();
//}
/// <summary>
/// Sets current Control Mode and establishes communication if applicable.
/// </summary>
/// <param name="mode"></param>
/// <returns></returns>
public bool SetControlMode(ControlType mode)
{
if (mode == ControlType.Execute)
{
logger.Warning($"Execute mode temporarily deactivated. Try 'online' instead, it's cooler ;) ControlMode reverted to {_controlMode}");
return(false);
}
_controlMode = mode;
return(ResetControl());
}
/// <summary>
/// Sets the current ConnectionManagerType.
/// </summary>
/// <param name="mode"></param>
/// <returns></returns>
public bool SetConnectionMode(ConnectionType mode)
{
if (_driver == null)
{
throw new Exception("Missing Driver object");
}
if (!_driver.AvailableConnectionTypes[mode])
{
logger.Warning($"This device's driver does not accept ConnectionType {mode}, ConnectionMode remains {this.connectionMode}");
return(false);
}
this.connectionMode = mode;
return(ResetControl());
}
// ╔╦╗╔═╗╔╦╗╦╔═╗╔╗╔ ╔═╗╔═╗╔╦╗╦╔═╗╔╗╔╔═╗
// ║║║║ ║ ║ ║║ ║║║║ ╠═╣║ ║ ║║ ║║║║╚═╗
// ╩ ╩╚═╝ ╩ ╩╚═╝╝╚╝ ╩ ╩╚═╝ ╩ ╩╚═╝╝╚╝╚═╝
/// <summary>
/// Apply Translation Action.
/// </summary>
/// <param name="action"></param>
/// <returns></returns>
public bool ApplyAction(ActionTranslation action)
{
Vector newPosition = new Vector();
if (action.relative)
{
// If user issued a relative action, make sure there are absolute values to work with. (This limitation is due to current lack of FK/IK solvers)
if (position == null || rotation == null)
{
logger.Warning($"Cannot apply \"{action}\", must provide absolute position values first before applying relative ones... ");
return(false);
}
if (referenceCS == ReferenceCS.World)
{
newPosition = position + action.translation;
}
else
{
//Vector worldVector = Vector.Rotation(action.translation, Rotation.Conjugate(this.rotation));
Vector worldVector = Vector.Rotation(action.translation, this.rotation);
newPosition = position + worldVector;
}
}
else
{
// Fail if issued abs movement without prior rotation info. (This limitation is due to current lack of FK/IK solvers)
if (rotation == null)
{
logger.Warning($"Cannot apply \"{action}\", currently missing TCP orientation to work with... ");
return(false);
}
newPosition.Set(action.translation);
}
// @TODO: this must be more programmatically implemented
//if (Control.SAFETY_CHECK_TABLE_COLLISION)
//{
// if (Control.IsBelowTable(newPosition.Z))
// {
// if (Control.SAFETY_STOP_ON_TABLE_COLLISION)
// {
// Console.WriteLine("Cannot perform action: too close to base XY plane --> TCP.z = {0}", newPosition.Z);
// return false;
// }
// else
// {
// Console.WriteLine("WARNING: too close to base XY plane, USE CAUTION! --> TCP.z = {0}", newPosition.Z);
// }
// }
//}
prevPosition = position;
position = newPosition;
prevRotation = rotation; // to flag same-orientation change
prevAxes = axes;
axes = null; // flag joints as null to avoid Joint instructions using obsolete data
if (isExtruding)
{
this.ComputeExtrudedLength();
}
if (_logRelativeActions && action.relative)
{
logger.Verbose("TCP position at " + this.position);
}
return(true);
}