public MotionUpdateArgs(Vector pos, Rotation ori, Joints axes, ExternalAxes extax)
{
this.Position = pos;
this.Rotation = ori;
this.Axes = axes;
this.ExternalAxes = extax;
}
// ╔═╗╔═╗╦═╗╔═╗╔═╗╔╦╗ ╦ ╦╔═╗╔╦╗╔═╗╔╦╗╔═╗╔═╗
// ╠╣ ║ ║╠╦╝║ ║╣ ║║ ║ ║╠═╝ ║║╠═╣ ║ ║╣ ╚═╗
// ╚ ╚═╝╩╚═╚═╝╚═╝═╩╝ ╚═╝╩ ═╩╝╩ ╩ ╩ ╚═╝╚═╝
/// <summary>
/// Force-update a full pose without going through Action application.
/// Temporarily here for MotionUpdate cursors, until I figure out a better way of dealing with it...
/// </summary>
/// <param name="pos"></param>
/// <param name="rot"></param>
/// <param name="ax"></param>
/// <param name="extax"></param>
internal void UpdateFullPose(Vector pos, Rotation rot, Joints ax, ExternalAxes extax)
{
this.position = pos;
this.rotation = rot;
this.axes = ax;
this.externalAxesCartesian = extax;
}
// ╔═╗═╗ ╦╔╦╗╔═╗╦═╗╔╗╔╔═╗╦ ╔═╗═╗ ╦╦╔═╗
// ║╣ ╔╩╦╝ ║ ║╣ ╠╦╝║║║╠═╣║ ╠═╣╔╩╦╝║╚═╗
// ╚═╝╩ ╚═ ╩ ╚═╝╩╚═╝╚╝╩ ╩╩═╝────╩ ╩╩ ╚═╩╚═╝
public bool ApplyAction(ActionExternalAxis action)
{
// Cartesian targets
if (action.target == ExternalAxesTarget.All || action.target == ExternalAxesTarget.Cartesian)
{
if (this.externalAxesCartesian == null)
{
this.externalAxesCartesian = new ExternalAxes();
}
if (action.relative)
{
if (this.externalAxesCartesian[action.axisNumber - 1] == null)
{
//logger.Info($"Cannot apply \"{action}\", must initialize absolute axis value first for axis {action.axisNumber} before applying relative ones...");
logger.Error("Cannot increase cartesian external axis, value has not been initialized. Try `ExternalAxisTo()` instead.");
return(false);
}
this.externalAxesCartesian[action.axisNumber - 1] += action.value;
}
else
{
this.externalAxesCartesian[action.axisNumber - 1] = action.value;
}
}
// Joint targets
if (action.target == ExternalAxesTarget.All || action.target == ExternalAxesTarget.Joint)
{
if (this.externalAxesJoints == null)
{
this.externalAxesJoints = new ExternalAxes();
}
if (action.relative)
{
if (this.externalAxesJoints[action.axisNumber - 1] == null)
{
//logger.Info($"Cannot apply \"{action}\", must initialize absolute axis value first for axis {action.axisNumber} before applying relative ones...");
logger.Error("Cannot increase joint external axis, value has not been initialized. Try `ExternalAxisTo()` instead.");
return(false);
}
this.externalAxesJoints[action.axisNumber - 1] += action.value;
}
else
{
this.externalAxesJoints[action.axisNumber - 1] = action.value;
}
}
if (_logRelativeActions && action.relative)
{
logger.Verbose("External Axis " + action.axisNumber + " set to " + this.externalAxesJoints[action.axisNumber - 1]);
}
return(true);
}
/// <summary>
/// Create a new instance as a copy.
/// </summary>
/// <param name="extAx"></param>
public ExternalAxes(ExternalAxes extAx)
{
for (int i = 0; i < extAx.Length; i++)
{
this._externalAxes[i] = extAx[i];
}
}
public ActionIssuedArgs(Action last, Vector pos, Rotation ori, Joints axes, ExternalAxes extax)
{
this.LastAction = last;
this.Position = pos;
this.Rotation = ori;
this.Axes = axes;
this.ExternalAxes = ExternalAxes;
}
public ActionReleasedArgs(Action last, int pendingReleaseToDevice, Vector pos, Rotation ori, Joints axes, ExternalAxes extax)
{
this.LastAction = last;
this.PendingReleaseToDevice = pendingReleaseToDevice;
this.Position = pos;
this.Rotation = ori;
this.Axes = axes;
this.ExternalAxes = ExternalAxes;
}
/// <summary>
/// Minimum information necessary to initialize a robot object.
/// </summary>
/// <param name="position"></param>
/// <param name="rotation"></param>
/// <param name="joints"></param>
/// <returns></returns>
public bool Initialize(Vector position, Rotation rotation, Joints joints, ExternalAxes extAx,
double speed, double acceleration, double precision, MotionType mType, ReferenceCS refCS)
{
if (position != null)
{
this.position = new Vector(position);
this.prevPosition = new Vector(position);
}
if (rotation != null)
{
this.rotation = new Rotation(rotation);
this.prevRotation = new Rotation(rotation);
}
if (joints != null)
{
this.axes = new Joints(joints);
this.prevAxes = new Joints(joints);
}
if (extAx != null)
{
// @TODO split this definition
this.externalAxesCartesian = new ExternalAxes(extAx);
this.externalAxesJoints = new ExternalAxes(extAx);
}
this.acceleration = acceleration;
this.speed = speed;
this.precision = precision;
this.motionType = mType;
this.referenceCS = refCS;
this.availableTools = new Dictionary <string, Tool>();
// Add a "noTool" default object and make it the default.
//this.availableTools["noTool"] = Tool.Create("noTool", 0, 0, 0, 1, 0, 0, 0, 1, 0, 0.001, 0, 0, 0);
//this.tool = this.availableTools["noTool"];
this.tool = null; // reverted back to default `null` tool...
this.digitalOutputs = new Dictionary <string, bool>();
this.analogOutputs = new Dictionary <string, double>();
// Initialize temps to zero
this.partTemperature = new Dictionary <RobotPartType, double>();
foreach (RobotPartType part in Enum.GetValues(typeof(RobotPartType)))
{
partTemperature[part] = 0;
}
isExtruding = false;
extrusionRate = 0;
extrudedLength = 0;
this.initialized = true;
return(this.initialized);
}
public ActionExecutedArgs(Action last, int pendingExecutionOnDevice, int pendingExecutionTotal, Vector pos, Rotation ori, Joints axes, ExternalAxes extax)
{
this.LastAction = last;
this.PendingExecutionOnDevice = pendingExecutionOnDevice;
this.PendingExecutionTotal = pendingExecutionTotal;
this.Position = pos;
this.Rotation = ori;
this.Axes = axes;
this.ExternalAxes = ExternalAxes;
}
internal void UpdateRobotStatus()
{
if (bot == null)
{
ResetRobotStatus();
return;
}
uiContext.Post(x =>
{
Machina.Vector pos = bot.GetCurrentPosition();
string posStr = pos?.ToString(true) ?? "-";
lbl_Status_TCP_Position_Value.Content = posStr;
Machina.Orientation ori = bot.GetCurrentRotation();
string oriStr = ori?.ToString(true) ?? "-";
lbl_Status_TCP_Orientation_Value.Content = oriStr;
Machina.Joints axes = bot.GetCurrentAxes();
string axesStr = axes?.ToString(true) ?? "-";
lbl_Status_Axes_Value.Content = axesStr;
Machina.ExternalAxes extax = bot.GetCurrentExternalAxes();
bool nullext = true;
if (extax != null)
{
for (int i = 0; i < 6; i++)
{
if (extax[i] != null)
{
nullext = false;
break;
}
}
}
lbl_Status_Ext_Axes_Value.Content = nullext ? "-" : extax.ToString(true);
double speed = bot.GetCurrentSpeed();
double acc = bot.GetCurrentAcceleration();
string speedacc = Math.Round(speed, Machina.Geometry.STRING_ROUND_DECIMALS_MM) + " mm/s / " + Math.Round(acc, Machina.Geometry.STRING_ROUND_DECIMALS_MM) + " mm/s^2";
lbl_Status_SpeedAcceleration_Value.Content = speedacc;
double precision = bot.GetCurrentPrecision();
lbl_Status_Precision_Value.Content =
Math.Round(precision, Machina.Geometry.STRING_ROUND_DECIMALS_MM) + " mm";
Machina.MotionType mtype = bot.GetCurrentMotionMode();
lbl_Status_MotionMode_Value.Content = mtype.ToString();
lbl_Status_Tool_Value.Content = bot.GetCurrentTool()?.name ?? "(no tool)";
}, null);
}
internal bool InitializeRobotCursors()
{
if (_driver == null)
{
throw new Exception("Cannot initialize Robotcursors without a _comm object");
}
// If successful, initialize robot cursors to mirror the state of the device
Vector currPos = _driver.GetCurrentPosition();
Rotation currRot = _driver.GetCurrentOrientation();
Joints currJnts = _driver.GetCurrentJoints();
ExternalAxes currExtAx = _driver.GetCurrentExternalAxes();
return(InitializeRobotCursors(currPos, currRot, currJnts, currExtAx));
}
/// <summary>
/// Gets the RAPID extax representation from an ExternalAxes object.
/// </summary>
/// <param name="extax"></param>
/// <returns></returns>
static internal string GetExternalAxesValue(ExternalAxes extax)
{
if (extax == null)
{
return("[9E9,9E9,9E9,9E9,9E9,9E9]");
}
string extj = "[";
double?val;
for (int i = 0; i < extax.Length; i++)
{
val = extax[i];
extj += (val == null) ? "9E9" : Math.Round((double)val, Geometry.STRING_ROUND_DECIMALS_MM).ToString(CultureInfo.InvariantCulture);
if (i < extax.Length - 1)
{
extj += ",";
}
}
extj += "]";
return(extj);
}
// ╔═╗═╗ ╦╔╦╗╔═╗╦═╗╔╗╔╔═╗╦ ╔═╗═╗ ╦╦╔═╗
// ║╣ ╔╩╦╝ ║ ║╣ ╠╦╝║║║╠═╣║ ╠═╣╔╩╦╝║╚═╗
// ╚═╝╩ ╚═ ╩ ╚═╝╩╚═╝╚╝╩ ╩╩═╝────╩ ╩╩ ╚═╩╚═╝
public bool ApplyAction(ActionExternalAxis action)
{
if (this.externalAxes == null)
{
this.externalAxes = new ExternalAxes();
}
if (action.relative)
{
if (this.externalAxes[action.axisNumber - 1] == null)
{
Console.WriteLine($"Sorry, must initialize absolute axis value first for axis {action.axisNumber} before applying relative ones... Action: " + action.ToInstruction());
return(false);
}
this.externalAxes[action.axisNumber - 1] += action.value;
}
else
{
this.externalAxes[action.axisNumber - 1] = action.value;
}
return(true);
}
///// <summary>
///// If there was a running Communication protocol, drop it and restart it again.
///// </summary>
///// <returns></returns>
//private bool ResetCommunication()
//{
// if (_driver == null)
// {
// Console.WriteLine("Communication protocol not established, please initialize first.");
// }
// DropCommunication();
// return InitializeCommunication();
//}
/// <summary>
/// Initializes all instances of robotCursors with base information
/// </summary>
/// <param name="position"></param>
/// <param name="rotation"></param>
/// <param name="joints"></param>
/// <returns></returns>
internal bool InitializeRobotCursors(Point position = null, Rotation rotation = null, Joints joints = null, ExternalAxes extAx = null,
double speed = Control.DEFAULT_SPEED, double acc = Control.DEFAULT_ACCELERATION, double precision = Control.DEFAULT_PRECISION,
MotionType mType = Control.DEFAULT_MOTION_TYPE, ReferenceCS refCS = Control.DEFAULT_REFCS)
{
bool success = true;
success &= IssueCursor.Initialize(position, rotation, joints, extAx, speed, acc, precision, mType, refCS);
success &= ReleaseCursor.Initialize(position, rotation, joints, extAx, speed, acc, precision, mType, refCS);
success &= ExecutionCursor.Initialize(position, rotation, joints, extAx, speed, acc, precision, mType, refCS);
_areCursorsInitialized = success;
return(success);
}
