/// <summary>
/// Returns a simple XYZ position.
/// </summary>
/// <param name="cursor"></param>
/// <returns></returns>
internal string GetPositionTargetValue(RobotCursor cursor)
{
return(string.Format("X{0} Y{1} Z{2}",
Math.Round(cursor.position.X, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.position.Y, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.position.Z, Geometry.STRING_ROUND_DECIMALS_MM)));
}
/// <summary>
/// Returns an RAPID jointtarget representation of the current state of the cursor.
/// </summary>
/// <returns></returns>
static internal string GetJointTargetValue(RobotCursor cursor)
{
return(string.Format(CultureInfo.InvariantCulture,
"[{0}, {1}]",
cursor.axes,
GetExternalJointsJointTargetValue(cursor)));
}
/// <summary>
/// Returns an RAPID robtarget representation of the current state of the cursor.
/// WARNING: this method is EXTREMELY UNSAFE; it performs no IK calculations, assigns default [0,0,0,0]
/// robot configuration and assumes the robot controller will figure out the correct one.
/// </summary>
/// <returns></returns>
static internal string GetUNSAFERobTargetValue(RobotCursor cursor)
{
return(string.Format("[{0}, {1}, {2}, {3}]",
cursor.position.ToString(false),
cursor.rotation.Q.ToString(false),
"[0,0,0,0]", // no IK at this moment
GetExternalJointsValue(cursor)));
}
/// <summary>
/// Main constructor.
/// </summary>
public ActionBuffer(RobotCursor parent)
{
this._parent = parent;
this.released = new List <Action>();
this.pending = new List <Action>();
this.blockCounts = new List <int>();
}
/// <summary>
/// Returns an RAPID robtarget representation of the current state of the cursor.
/// WARNING: this method is EXTREMELY UNSAFE; it performs no IK calculations, assigns default [0,0,0,0]
/// robot configuration and assumes the robot controller will figure out the correct one.
/// </summary>
/// <returns></returns>
static internal string GetUNSAFERobTargetValue(RobotCursor cursor)
{
return(string.Format("[{0}, {1}, {2}, {3}]",
cursor.position.ToString(false),
cursor.rotation.Q.ToString(false),
"[0,0,0,0]", // no IK at this moment
"[0,9E9,9E9,9E9,9E9,9E9]")); // no external axes at this moment
}
public bool Push(RobotCursor cursor)
{
if (buffer.Count >= limit)
{
throw new Exception("TOO MANY PUSHES WITHOUT POPS?");
}
buffer.Add(cursor.GetSettings());
return(true);
}
// ╔╗ ╔═╗╔═╗╔═╗
// ╠╩╗╠═╣╚═╗║╣
// ╚═╝╩ ╩╚═╝╚═╝
/// <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.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();
//// Basics io names
//for (int i = 0; i < digitalOutputNames.Length; i++)
//{
// digitalOutputNames[i] = "do" + i;
//}
//for (int i = 0; i < digitalOutputNames.Length; i++)
//{
// analogOutputNames[i] = "ao" + i;
//}
// Initialize temps to zero
foreach (RobotPartType part in Enum.GetValues(typeof(RobotPartType)))
{
partTemperature[part] = 0;
}
isExtruding = false; // should these go into Initilize()?
extrusionRate = 0;
extrudedLength = 0;
}
public bool Push(RobotCursor cursor)
{
if (buffer.Count >= limit)
{
cursor.logger.Error("Too many Pushes without Pops?");
throw new Exception("TOO MANY PUSHES WITHOUT POPS?");
}
buffer.Add(cursor.GetSettings());
return(true);
}
/// <summary>
/// Returns a KRL AXIS joint representation of the current state of the cursor.
/// </summary>
/// <returns></returns>
internal string GetAxisTargetValue(RobotCursor cursor)
{
return(string.Format("{{AXIS: A1 {0}, A2 {1}, A3 {2}, A4 {3}, A5 {4}, A6 {5}}}",
Math.Round(cursor.joints.J1, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J2, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J3, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J4, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J5, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(cursor.joints.J6, Geometry.STRING_ROUND_DECIMALS_DEGS)));
}
/// <summary>
/// Returns a RAPID representation of cursor speed.
/// </summary>
/// <param name="speed"></param>
/// <returns></returns>
static internal string GetSpeedValue(RobotCursor cursor)
{
// Default speed declarations in ABB always use 500 deg/s as rot speed, but it feels too fast (and scary).
// Using either rotationSpeed value or the same value as lin motion here.
return(string.Format("[{0},{1},{2},{3}]",
cursor.speed,
cursor.rotationSpeed > Geometry.EPSILON2 ? cursor.rotationSpeed : cursor.speed,
5000,
1000));
}
public Settings Pop(RobotCursor cursor)
{
if (buffer.Count > 0)
{
_settingsBeforeLastPopped = cursor.GetSettings();
_lastPoppped = buffer.Last();
buffer.RemoveAt(buffer.Count - 1);
return(_lastPoppped);
}
return(null);
}
/// <summary>
/// Gets the cursors extax representation for a Cartesian target.
/// </summary>
/// <param name="cursor"></param>
/// <returns></returns>
static internal string GetExternalJointsRobTargetValue(RobotCursor cursor)
{
// If user initializes arm-angle, this extax is just the arm-angle value
if (cursor.armAngle != null)
{
return(GetExternalAxesValue(new ExternalAxes(cursor.armAngle)));
}
// Otherwise, use externalAxes
return(GetExternalAxesValue(cursor.externalAxesCartesian));
}
/// <summary>
/// Returns an UR pose representation of the current state of the cursor.
/// </summary>
/// <returns></returns>
internal static string GetPoseTargetValue(RobotCursor cursor)
{
RotationVector axisAng = cursor.rotation.GetRotationVector(true);
return(string.Format("p[{0},{1},{2},{3},{4},{5}]",
Math.Round(0.001 * cursor.position.X, Geometry.STRING_ROUND_DECIMALS_M),
Math.Round(0.001 * cursor.position.Y, Geometry.STRING_ROUND_DECIMALS_M),
Math.Round(0.001 * cursor.position.Z, Geometry.STRING_ROUND_DECIMALS_M),
Math.Round(axisAng.X, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(axisAng.Y, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(axisAng.Z, Geometry.STRING_ROUND_DECIMALS_RADS)));
}
public override RobotProgram UNSAFEFullProgramFromBuffer(string programName, RobotCursor writer, bool block, bool inlineTargets, bool humanComments)
{
// The program files to be returned
RobotProgram robotProgram = new RobotProgram(programName, CC);
// Which pending Actions are used for this program?
// Copy them without flushing the buffer.
List <Action> actions = block ?
writer.actionBuffer.GetBlockPending(false) :
writer.actionBuffer.GetAllPending(false);
// ACTION LINES GENERATION
List <string> actionLines = new List <string>();
// DATA GENERATION
// Use the write RobotCursor to generate the data
int it = 0;
string line = null;
foreach (Action a in actions)
{
// Move writerCursor to this action state
writer.ApplyNextAction(); // for the buffer to correctly manage them
line = string.Format("[{0}] {1}", it, a.ToString());
actionLines.Add(line);
// Move on
it++;
}
// PROGRAM ASSEMBLY
// Initialize a module list
List <string> module = new List <string>();
// Banner
module.AddRange(GenerateDisclaimerHeader(programName));
module.Add("");
// Code lines
module.AddRange(actionLines);
// MAIN file
RobotProgramFile pFile = new RobotProgramFile(programName, "txt", Encoding, CC);
pFile.SetContent(module);
robotProgram.Add(pFile);
return(robotProgram);
}
//// @TODO: this will need to get reallocated when fixing stream mode...
//public StreamQueue streamQueue;
//██████╗ ██╗ ██╗██████╗ ██╗ ██╗ ██████╗
//██╔══██╗██║ ██║██╔══██╗██║ ██║██╔════╝
//██████╔╝██║ ██║██████╔╝██║ ██║██║
//██╔═══╝ ██║ ██║██╔══██╗██║ ██║██║
//██║ ╚██████╔╝██████╔╝███████╗██║╚██████╗
//╚═╝ ╚═════╝ ╚═════╝ ╚══════╝╚═╝ ╚═════╝
/// <summary>
/// Main constructor.
/// </summary>
public Control(Robot parentBot)
{
parentRobot = parentBot;
// Reset();
motionCursor = new RobotCursor(this, "motionCursor", false, null);
writeCursor = new RobotCursor(this, "writeCursor", false, motionCursor);
virtualCursor = new RobotCursor(this, "virtualCursor", true, writeCursor);
SetControlMode(DEFAULT_CONTROLMODE);
SetConnectionMode(DEFAULT_CONNECTIONMODE);
}
/// <summary>
/// Returns a RAPID representatiton of cursor zone.
/// </summary>
/// <param name="cursor"></param>
/// <returns></returns>
static internal string GetZoneValue(RobotCursor cursor)
{
if (cursor.precision < Geometry.EPSILON2)
{
return("fine");
}
// Following conventions for default RAPID zones.
double high = 1.5 * cursor.precision;
double low = 0.10 * cursor.precision;
return(string.Format("[FALSE,{0},{1},{2},{3},{4},{5}]", cursor.precision, high, high, low, high, low));
}
/// <summary>
/// Returns a KRL FRAME representation of the current state of the cursor.
/// Note POS also accept T and S parameters for unambiguous arm configuration def. @TODO: implement?
/// </summary>
/// <returns></returns>
internal string GetPositionTargetValue(RobotCursor cursor)
{
YawPitchRoll euler = cursor.rotation.Q.ToYawPitchRoll(); // @TODO: does this actually work...?
return(string.Format("{{POS: X {0}, Y {1}, Z {2}, A {3}, B {4}, C {5}}}",
Math.Round(cursor.position.X, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.position.Y, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.position.Z, Geometry.STRING_ROUND_DECIMALS_MM),
// note reversed ZYX order
Math.Round(euler.ZAngle, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(euler.YAngle, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(euler.XAngle, Geometry.STRING_ROUND_DECIMALS_DEGS)));
}
/// <summary>
/// Returns a UR joint representation of the current state of the cursor.
/// </summary>
/// <returns></returns>
internal static string GetJointTargetValue(RobotCursor cursor)
{
Joints jrad = new Joints(cursor.joints); // use a shallow copy
jrad.Scale(Geometry.TO_RADS); // convert to radians
return(string.Format("[{0},{1},{2},{3},{4},{5}]",
Math.Round(jrad.J1, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(jrad.J2, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(jrad.J3, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(jrad.J4, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(jrad.J5, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(jrad.J6, Geometry.STRING_ROUND_DECIMALS_RADS)));
}
//// @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>
/// Returns a RAPID representation of a Tool object.
/// </summary>
/// <param name="cursor"></param>
/// <returns></returns>
static internal string GetToolValue(RobotCursor cursor) //TODO: wouldn't it be just better to pass the Tool object? Inconsistent with the rest of the API...
{
if (cursor.tool == null)
{
throw new Exception("Cursor has no tool attached");
}
return(string.Format("[TRUE, [{0},{1}], [{2},{3},{4},0,0,0]]",
cursor.tool.TCPPosition,
cursor.tool.TCPOrientation.Q.ToString(false),
cursor.tool.Weight,
cursor.tool.centerOfGravity,
"[1,0,0,0]")); // no internial axes by default
}
/// <summary>
/// Computes how much the cursor has moved in this action, and returns how much
/// filament it should extrude based on extrusion rate.
/// </summary>
/// <param name="cursor"></param>
/// <param name="action"></param>
/// <returns></returns>
internal string GetExtrusionTargetValue(RobotCursor cursor)
{
double len = cursor.extrudedLength - this.extrusionLengthResetPosition;
// If extruded over the limit, reset extrude position and start over
if (len > extrusionLengthResetEvery)
{
this.instructionLines.Add($"{commChar} Homing extrusion length after {cursor.prevExtrudedLength - this.extrusionLengthResetPosition} mm ({this.extrusionLengthResetEvery} mm limit)");
this.instructionLines.Add($"G92 E0.0000");
this.extrusionLengthResetPosition = cursor.prevExtrudedLength;
len = cursor.extrudedLength - this.extrusionLengthResetPosition;
}
return($"E{Math.Round(len, 5)}");
}
/// <summary>
/// Returns a RAPID representation of cursor speed.
/// </summary>
/// <param name="speed"></param>
/// <returns></returns>
static internal string GetSpeedValue(RobotCursor cursor)
{
// ABB format: [TCP linear speed in mm/s, TCP reorientation speed in deg/s, linear external axis speed in mm/s, rotational external axis speed in deg/s]
// Default linear speeddata are [vel, 500, 5000, 1000], which feels like a lot.
// Just use the speed data as linear or rotational value, and stay safe.
string vel = Math.Round(cursor.speed, Geometry.STRING_ROUND_DECIMALS_MM).ToString(CultureInfo.InvariantCulture);
return(string.Format("[{0},{1},{2},{3}]", vel, vel, vel, vel));
//// Default speed declarations in ABB always use 500 deg/s as rot speed, but it feels too fast (and scary).
//// Using either rotationSpeed value or the same value as lin motion here.
//return string.Format("[{0},{1},{2},{3}]",
// cursor.speed,
// cursor.rotationSpeed > Geometry.EPSILON2 ? cursor.rotationSpeed : cursor.speed,
// 5000,
// 1000);
}
/// <summary>
/// Returns a RAPID representation of an extjoints object
/// </summary>
/// <param name="cursor"></param>
/// <returns></returns>
static internal string GetExternalJointsValue(RobotCursor cursor)
{
string extj = "[";
double?val;
for (int i = 0; i < cursor.externalAxes.Length; i++)
{
val = cursor.externalAxes[i];
extj += (val == null) ? "9E9" : val.ToString();
if (i < cursor.externalAxes.Length - 1)
{
extj += ",";
}
}
extj += "]";
return(extj);
}
/// <summary>
/// Computes how much the cursor has moved in this action, and returns how much
/// filament it should extrude based on extrusion rate.
/// </summary>
/// <param name="cursor"></param>
/// <param name="action"></param>
/// <returns></returns>
internal string GetExtrusionTargetValue(RobotCursor cursor)
{
double len = cursor.extrudedLength - this.extrusionLengthResetPosition;
// If extruded over the limit, reset extrude position and start over
if (len > extrusionLengthResetEvery)
{
this.instructionLines.Add($"{CC} Homing extrusion length after {cursor.prevExtrudedLength - this.extrusionLengthResetPosition} mm ({this.extrusionLengthResetEvery} mm limit)");
this.instructionLines.Add($"G92 E0.0000");
this.extrusionLengthResetPosition = cursor.prevExtrudedLength;
len = cursor.extrudedLength - this.extrusionLengthResetPosition;
}
return(string.Format(CultureInfo.InvariantCulture,
"E{0}",
Math.Round(len, 5)));
}
/// <summary>
/// Returns a UR representation of a Tool object.
/// </summary>
/// <param name="cursor"></param>
/// <returns></returns>
internal static string GetToolValue(RobotCursor cursor) //TODO: wouldn't it be just better to pass the Tool object? Inconsistent with the rest of the API...
{
if (cursor.tool == null)
{
throw new Exception("Cursor has no tool attached");
}
RotationVector axisAng = cursor.tool.TCPOrientation.Q.ToRotationVector(true);
return(string.Format("p[{0},{1},{2},{3},{4},{5}]",
Math.Round(0.001 * cursor.tool.TCPPosition.X, Geometry.STRING_ROUND_DECIMALS_M),
Math.Round(0.001 * cursor.tool.TCPPosition.Y, Geometry.STRING_ROUND_DECIMALS_M),
Math.Round(0.001 * cursor.tool.TCPPosition.Z, Geometry.STRING_ROUND_DECIMALS_M),
Math.Round(axisAng.X, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(axisAng.Y, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(axisAng.Z, Geometry.STRING_ROUND_DECIMALS_RADS)));
}
/// <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>
/// Returns a KRL representation of a Tool object
/// </summary>
/// <param name="cursor"></param>
/// <returns></returns>
internal string GetToolValue(RobotCursor cursor)
{
if (cursor.tool == null)
{
throw new Exception("Cursor has no tool attached");
}
YawPitchRoll euler = cursor.tool.TCPOrientation.Q.ToYawPitchRoll();
return(string.Format("{{X {0}, Y {1}, Z {2}, A {3}, B {4}, C {5}}}",
Math.Round(cursor.tool.TCPPosition.X, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.tool.TCPPosition.Y, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.tool.TCPPosition.Z, Geometry.STRING_ROUND_DECIMALS_MM),
// note reversed ZYX order
Math.Round(euler.ZAngle, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(euler.YAngle, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(euler.XAngle, Geometry.STRING_ROUND_DECIMALS_DEGS)));
}
/// <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 override List <string> UNSAFEProgramFromBuffer(string programName, RobotCursor writer, bool block, bool inlineTargets, bool humanComments)
{
// Which pending Actions are used for this program?
// Copy them without flushing the buffer.
List <Action> actions = block ?
writer.actionBuffer.GetBlockPending(false) :
writer.actionBuffer.GetAllPending(false);
// ACTION LINES GENERATION
List <string> actionLines = new List <string>();
// DATA GENERATION
// Use the write RobotCursor to generate the data
int it = 0;
string line = null;
foreach (Action a in actions)
{
// Move writerCursor to this action state
writer.ApplyNextAction(); // for the buffer to correctly manage them
line = a.ToInstruction();
actionLines.Add(line);
// Move on
it++;
}
// PROGRAM ASSEMBLY
// Initialize a module list
List <string> module = new List <string>();
// Banner
module.AddRange(GenerateDisclaimerHeader(programName));
module.Add("");
// Code lines
module.AddRange(actionLines);
return(module);
}
// ╦ ╦╔╦╗╦╦ ╔═╗
// ║ ║ ║ ║║ ╚═╗
// ╚═╝ ╩ ╩╩═╝╚═╝
internal bool GenerateVariableDeclaration(Action action, RobotCursor cursor, int id, out string declaration)
{
string dec = null;
switch (action.Type)
{
case ActionType.Translation:
case ActionType.Rotation:
case ActionType.Transformation:
dec = string.Format(" CONST robtarget target{0} := {1};", id, GetRobTargetValue(cursor));
break;
case ActionType.Axes:
dec = string.Format(" CONST jointtarget target{0} := {1};", id, GetJointTargetValue(cursor));
break;
}
declaration = dec;
return(dec != null);
}
