public Target(Tool tool, Speed speed, Zone zone, Command command, Frame frame = null, IEnumerable<double> external = null)
{
this.Tool = tool ?? Tool.Default;
this.Speed = speed ?? Speed.Default;
this.Zone = zone ?? Zone.Default;
this.Frame = frame ?? Frame.Default;
this.Command = command;
this.External = (external != null) ? external.ToArray() : new double[0];
}
public JointTarget(double[] joints, Tool tool = null, Speed speed = null, Zone zone = null, Command command = null, Frame frame = null, IEnumerable<double> external = null) : base(tool, speed, zone, command, frame, external)
{
this.Joints = joints;
}
static Zone()
{
Default = new Zone(0, "DefaultZone");
}
public CartesianTarget(Plane plane, RobotConfigurations? configuration = null, Motions motion = Motions.Joint, Tool tool = null, Speed speed = null, Zone zone = null, Command command = null, Frame frame = null, IEnumerable<double> external = null) : base(tool, speed, zone, command, frame, external)
{
this.Plane = plane;
this.Motion = motion;
this.Configuration = configuration;
}
string Zone(Zone zone)
{
double angle = zone.Rotation.ToDegrees();
return $"TASK PERS zonedata {zone.Name}:=[FALSE,{zone.Distance:0.00},{zone.Distance:0.00},{zone.Distance:0.00},{angle:0.00},{zone.Distance:0.00},{angle:0.00}];";
}
List <string> SrcFile(int file, int group)
{
string groupName = cell.MechanicalGroups[group].Name;
int start = program.MultiFileIndices[file];
int end = (file == program.MultiFileIndices.Count - 1) ? program.Targets.Count : program.MultiFileIndices[file + 1];
var code = new List <string>
{
$@"&ACCESS RVP
&REL 1
DEF {program.Name}_{groupName}_{file:000}()
"
};
Tool currentTool = null;
Frame currentFrame = null;
Speed currentSpeed = null;
double currentPercentSpeed = 0;
Zone currentZone = null;
for (int j = start; j < end; j++)
{
var cellTarget = program.Targets[j];
var programTarget = cellTarget.ProgramTargets[group];
var target = programTarget.Target;
if (currentTool == null || target.Tool != currentTool)
{
code.Add(SetTool(target.Tool));
currentTool = target.Tool;
}
if (currentFrame == null || target.Frame != currentFrame)
{
if (target.Frame.IsCoupled)
{
int mech = target.Frame.CoupledMechanism + 2;
code.Add($@"$BASE = EK(MACHINE_DEF[{mech}].ROOT, MACHINE_DEF[{mech}].MECH_TYPE, {target.Frame.Name})");
code.Add($@"$ACT_EX_AX = 2");
}
else
{
code.Add($"$BASE={target.Frame.Name}");
}
currentFrame = target.Frame;
}
if (target.Zone.IsFlyBy && (currentZone == null || target.Zone != currentZone))
{
code.Add($"$APO.CDIS={target.Zone.Name}");
currentZone = target.Zone;
}
if (programTarget.Index > 0)
{
if (programTarget.LeadingJoint > 5)
{
code.Add(ExternalSpeed(programTarget));
}
else
{
if (currentSpeed == null || target.Speed != currentSpeed)
{
if (!programTarget.IsJointMotion)
{
double rotation = target.Speed.RotationSpeed.ToDegrees();
// code.Add($"$VEL={{CP {target.Speed.Name}, ORI1 {rotation:0.000}, ORI2 {rotation:0.000}}}");
code.Add($"$VEL.CP = {target.Speed.Name}\r\n$VEL.ORI1 = {rotation:0.000}\r\n$VEL.ORI2 = {rotation:0.000}");
currentSpeed = target.Speed;
}
}
if (programTarget.IsJointMotion)
{
double percentSpeed = cellTarget.MinTime / cellTarget.DeltaTime;
if (Abs(currentPercentSpeed - percentSpeed) > UnitTol)
{
code.Add($"BAS(#VEL_PTP, 100)");
if (cellTarget.DeltaTime > UnitTol)
{
code.Add($"$VEL_AXIS[{programTarget.LeadingJoint + 1}] = {percentSpeed * 100:0.000}");
}
currentPercentSpeed = percentSpeed;
}
}
}
}
// external axes
string external = string.Empty;
double[] values = cell.MechanicalGroups[group].RadiansToDegreesExternal(target);
for (int i = 0; i < target.External.Length; i++)
{
int num = i + 1;
external += $", E{num} {values[i]:0.000}";
}
// motion command
string moveText = null;
if (programTarget.IsJointTarget)
{
var jointTarget = target as JointTarget;
double[] jointDegrees = jointTarget.Joints.Select((x, i) => cell.MechanicalGroups[group].Robot.RadianToDegree(x, i)).ToArray();
moveText = $"PTP {{A1 {jointDegrees[0]:0.000},A2 {jointDegrees[1]:0.000},A3 {jointDegrees[2]:0.000},A4 {jointDegrees[3]:0.000},A5 {jointDegrees[4]:0.000},A6 {jointDegrees[5]:0.000}{external}}}";
if (target.Zone.IsFlyBy)
{
moveText += " C_PTP";
}
}
else
{
var cartesian = target as CartesianTarget;
var plane = cartesian.Plane;
var euler = PlaneToEuler(plane);
switch (cartesian.Motion)
{
case Target.Motions.Joint:
{
string bits = string.Empty;
// if (target.ChangesConfiguration)
{
double[] jointDegrees = programTarget.Kinematics.Joints.Select((x, i) => cell.MechanicalGroups[group].Robot.RadianToDegree(x, i)).ToArray();
int turnNum = 0;
for (int i = 0; i < 6; i++)
{
if (jointDegrees[i] < 0)
{
turnNum += (int)Pow(2, i);
}
}
var configuration = programTarget.Kinematics.Configuration;
bool shoulder = configuration.HasFlag(Target.RobotConfigurations.Shoulder);
bool elbow = configuration.HasFlag(Target.RobotConfigurations.Elbow);
elbow = !elbow;
bool wrist = configuration.HasFlag(Target.RobotConfigurations.Wrist);
int configNum = 0;
if (shoulder)
{
configNum += 1;
}
if (elbow)
{
configNum += 2;
}
if (wrist)
{
configNum += 4;
}
string status = Convert.ToString(configNum, 2);
string turn = Convert.ToString(turnNum, 2);
bits = $@", S'B{status:000}',T'B{turn:000000}'";
}
moveText = $"PTP {{X {euler[0]:0.00},Y {euler[1]:0.00},Z {euler[2]:0.00},A {euler[3]:0.000},B {euler[4]:0.000},C {euler[5]:0.000}{external}{bits}}}";
if (target.Zone.IsFlyBy)
{
moveText += " C_PTP";
}
break;
}
case Target.Motions.Linear:
{
moveText = $"LIN {{X {euler[0]:0.00},Y {euler[1]:0.00},Z {euler[2]:0.00},A {euler[3]:0.000},B {euler[4]:0.000},C {euler[5]:0.000}{external}}}";
if (target.Zone.IsFlyBy)
{
moveText += " C_DIS";
}
break;
}
}
}
code.Add(moveText);
foreach (var command in programTarget.Commands)
{
code.Add(command.Code(cell, target));
}
}
code.Add("END");
return(code);
}
string Zone(Zone zone)
{
double angle = zone.Rotation.ToDegrees();
return($"TASK PERS zonedata {zone.Name}:=[FALSE,{zone.Distance:0.00},{zone.Distance:0.00},{zone.Distance:0.00},{angle:0.00},{zone.Distance:0.00},{angle:0.00}];");
}