void CreateTargets(List <List <SimpleTarget> > paths)
{
double totalDistance = 0;
var externalCustom = new[] { "motorValue" };
_targets.Add(HomeStart());
foreach (var path in paths)
{
var first = path[0].Location;
var last = path[path.Count - 1].Location;
var firstSafe = first; firstSafe.Origin += firstSafe.Normal * _att.SafeZOffset;
_targets.Add(CreateTarget(firstSafe, _att.SafeSpeed, _att.SafeZone, 0));
_targets.Add(CreateTarget(first, _att.ApproachSpeed, _att.ApproachZone, 0));
_targets.Add(CreateTarget(first, _att.ApproachSpeed, _att.ApproachZone, _startDistance));
for (int i = 1; i < path.Count; i++)
{
//double segmentLength = path[i - 1].DistanceTo(path[i]);
Plane position = path[i].Location;
double segmentLength = path[i].Length;
var zone = i != path.Count - 1 ? _att.ExtrusionZone : _att.ApproachZone;
_targets.Add(CreateTarget(position, _att.ExtrusionSpeed, zone, segmentLength));
}
_targets.Add(CreateTarget(last, _att.ExtrusionSpeed, _att.ApproachZone, _suckBack));
var lastOffset = last; lastOffset.Origin += lastOffset.Normal * (_att.SafeZOffset + _att.LayerHeight);
_targets.Add(CreateTarget(lastOffset, _att.ApproachSpeed, _att.SafeZone, 0));
SubPrograms.Add(_targets.Count);
}
_targets.Add(HomeEnd());
SubPrograms.RemoveAt(SubPrograms.Count - 1);
Target CreateTarget(Plane location, Speed speed, Zone zone, double externalDistance)
{
var frame = _att.Frame;
var tool = _att.Tool;
totalDistance += externalDistance * _extrusionFactor;
Command command = null;
if (externalDistance != 0)
{
string sign = externalDistance < 0 ? "+" : "-";
string code = $"motorValue:=motorValue{sign}{Abs(externalDistance):0.000}*extrusionFactor;";
var externalCommand = new Robots.Commands.Custom($"SetExternal{_targets.Count}", command: code);
externalCommand.RunBefore = true;
command = externalCommand;
}
var target = new CartesianTarget(location, null, Motions.Linear, tool, speed, zone, command, frame, new[] { totalDistance });
target.ExternalCustom = externalCustom;
return(target);
}
Target HomeStart()
{
var externalValue = ExternalValue(_loopDistance);
string declaration = $@"VAR num motorValue:= 0;
PERS num extrusionFactor:={_extrusionFactor: 0.000};
VAR robtarget current;
VAR num choice:=0;
";
string initCode = $@"current:= CRobT(\Tool:= {_att.Tool.Name} \WObj:= {_att.Frame.Name});
EOffsSet current.extax;";
string testCode = $@"TPReadFK choice,""Choose mode"",stEmpty,stEmpty,stEmpty,""Program"",""Test"";
current:= CRobT(\Tool:= {_att.Tool.Name} \WObj:= {_att.Frame.Name});
WHILE choice = 5 DO
motorValue:= motorValue - {externalValue:0.00}*extrusionFactor;
current.extax.eax_a:= motorValue;
MoveL Offs(current,0,{_loopDistance},0),{_att.ExtrusionSpeed.Name},{_att.ExtrusionZone.Name},{_att.Tool.Name} \WObj:= {_att.Frame.Name};
motorValue:= motorValue - {externalValue:0.00}*extrusionFactor;
current.extax.eax_a:= motorValue;
MoveL Offs(current,0,0,0),{_att.ExtrusionSpeed.Name},{_att.ExtrusionZone.Name},{_att.Tool.Name} \WObj:= {_att.Frame.Name};
ENDWHILE";
var initCommand = new Robots.Commands.Custom("Init", declaration: declaration, command: initCode);
initCommand.RunBefore = true;
var testCommand = new Robots.Commands.Custom("Test", command: testCode);
var command = new Group(new[] { initCommand, testCommand });
var home = new JointTarget(_att.Home, _att.Tool, _att.SafeSpeed, _att.SafeZone, command, _att.Frame, new[] { totalDistance });
home.ExternalCustom = externalCustom;
return(home);
}
Target HomeEnd()
{
var command = new Group()
{
new Message("Se acabó."),
new Stop()
};
var home = new JointTarget(_att.Home, _att.Tool, _att.SafeSpeed, _att.SafeZone, command, _att.Frame, new[] { totalDistance });
home.ExternalCustom = externalCustom;
return(home);
}
}
public static IToolpath AddExtruderCommands(IToolpath toolpath, double externalFactor, string indMechanism = null)
{
if (toolpath == null)
{
return(toolpath);
}
var resetCommand = ResetCommand(toolpath.Targets.First());
var outTargets = SetExternalWithVariable(toolpath.Targets);
return(toolpath.ShallowClone(outTargets));
Command ResetCommand(Target refTarget)
{
string declaration = $@"VAR num motorValue:= 0;
PERS num extrusionFactor:= {externalFactor:0.000};
VAR robtarget current;
";
string resetCode = $@"current:= CRobT(\Tool:= {refTarget.Tool.Name} \WObj:= {refTarget.Frame.Name});
EOffsSet current.extax;
motorValue:= 0;";
string initCode;
if (indMechanism != null)
{
string indCode = $@"IndReset {indMechanism},1 \RefNum:=0 \Short;";
initCode = $"{indCode}\r\n{resetCode}";
}
else
{
initCode = resetCode;
}
var command = new Robots.Commands.Custom("ResetExtruder", declaration: declaration, command: initCode);
command.RunBefore = true;
return(command);
}
List <Target> SetExternalWithVariable(IEnumerable <Target> inTargets)
{
var outTargets = new List <Target>();
double totalDistance = 0;
int count = 0;
int i = 0;
Target prev = null;
foreach (var target in inTargets)
{
var current = target.ShallowClone();
double externalDistance = 0;
if (target.External.Length > 0)
{
externalDistance = target.External[0];
}
totalDistance += externalDistance;
current.External = new[] { totalDistance };
current.ExternalCustom = new[] { "motorValue" };
if (i == 0)
{
current.AppendCommand(resetCommand);
}
if (externalDistance != 0)
{
//if (!IsExtrusion(prev))
string sign = externalDistance < 0 ? "+" : "-";
string code = $"motorValue:=motorValue{sign}{Abs(externalDistance):0.000}*extrusionFactor;";
var externalCommand = new Robots.Commands.Custom($"SetExternal{count++}", command: code);
externalCommand.RunBefore = true;
current.AppendCommand(externalCommand);
}
outTargets.Add(current);
prev = target;
i++;
}
return(outTargets);
}
}
