public StepsPosition ToStepsPosition(UnitsPosition current, double feedrate, bool fastMovement)
{
// Delta Position & Delta Steps -> with this movement
double xDelta = Math.Abs(this.X - current.X);
double yDelta = Math.Abs(this.Y - current.Y);
double zDelta = Math.Abs(this.Z - current.Z);
// creamos el vector EN PASOS del movimiento que va a hacer la punta
StepsPosition lineSteps = new StepsPosition(
Convert.ToInt64(Math.Ceiling(this.X * Configuration.configValueX)),
Convert.ToInt64(Math.Ceiling(this.Y * Configuration.configValueY)),
Convert.ToInt64(Math.Ceiling(this.Z * Configuration.configValueZ)));
if (fastMovement)
{
lineSteps.Delay = 1;
return lineSteps;
}
// delay parameters
double totalDistance = Math.Sqrt(xDelta * xDelta + yDelta * yDelta + zDelta * zDelta);
//if (curveLength == -1) curveLength = totalDistance * totalDistance; // To handle lines and curveSections
long maxDeltaSteps = Convert.ToInt64(Math.Ceiling(Math.Max(xDelta * Configuration.configValueX, yDelta * Configuration.configValueY)));
maxDeltaSteps = Convert.ToInt64(Math.Ceiling(Math.Max(maxDeltaSteps, zDelta * Configuration.configValueZ)));
// Calculating Delay: totalDistance * 1 second(microseconds) / speed / maxDelta / 2-changeStates per step / milliseconds
if (maxDeltaSteps > 0)
{
lineSteps.Delay = Convert.ToInt64(Math.Ceiling( totalDistance * 60000000.0 / feedrate / maxDeltaSteps / 2 / 1000)); // time between clock for most dinamyc axis - microseconds
}
else
{
lineSteps.Delay = 0;
}
return lineSteps;
}
private UnitsPosition GetCenterPosition(string command)
{
UnitsPosition result = new UnitsPosition();
if (HasValueParameter('R', command))
{
double r = GetValueParameter('R', command);
result = GetFromRadius(r, command);
}
else
{
// The center point is relative to the start position... Translate to absolute programming mode
result.X = GetValueParameter('I', command) + this.CurrentPosition.X;
result.Y = GetValueParameter('J', command) + this.CurrentPosition.Y;
result.Z = GetValueParameter('K', command) + this.CurrentPosition.Z;
}
return result;
}
public StepsPosition ToStepsPosition(UnitsPosition current, double feedrate)
{
return ToStepsPosition(current, feedrate, false);
}
private List<UnitsPosition> ProcessCurvePlaneYZ(string code)
{
List<UnitsPosition> result = new List<UnitsPosition>();
double feedRate = HasValueParameter('F', code) ? GetValueParameter('F', code) : Configuration.defaultFeedrate; // Ver qué valor va cuando no hay valor - CONFIGURACION? -
UnitsPosition startPosition = this.CurrentPosition;
UnitsPosition finalPosition = this.GetFinalPosition(code);
UnitsPosition centerPosition = this.GetCenterPosition(code);
int gCode = Convert.ToInt32(GetValueParameter('G', code));
bool clockwise = gCode == 2 ? true : false;
double aY, aZ, bY, bZ, angleA, angleB, angle, radius, length;
int steps;
aY = currentPosition.Y - centerPosition.Y;
aZ = currentPosition.Z - centerPosition.Z;
bY = finalPosition.Y - centerPosition.Y;
bZ = finalPosition.Z - centerPosition.Z;
angleA = clockwise ? Math.Atan2(bZ, bY) : Math.Atan2(aZ, aY);
angleB = clockwise ? Math.Atan2(aZ, aY) : Math.Atan2(bZ, bY);
if (angleB <= angleA) angleB += 2 * Math.PI;
angle = angleB - angleA;
radius = Math.Sqrt(aY * aY + aZ * aZ);
length = radius * angle;
steps = (int)(length / Configuration.millimetersCurveSection);
for (int s = 1; s <= steps; s++)
{
UnitsPosition sectionPosition = new UnitsPosition();
int step = clockwise ? steps - s : s;
sectionPosition.X = (finalPosition.X - startPosition.X) * ((float)s / steps);
sectionPosition.Y = centerPosition.Y + radius * Math.Cos(angleA + angle * ((float)step / steps));
sectionPosition.Z = centerPosition.Z + radius * Math.Sin(angleA + angle * ((float)step / steps));
if ((sectionPosition.X < 0) || (sectionPosition.Y < 0) || (sectionPosition.Z < 0))
{
logger.Error("Underflow procesando: " + code + ".");
throw new Exception("Underflow procesando: " + code + ".");
}
result.Add(sectionPosition);
}
return result;
}
private List<UnitsPosition> ProcessCurvePlaneXY(string code)
{
List<UnitsPosition> result = new List<UnitsPosition>();
double feedRate = HasValueParameter('F', code) ? GetValueParameter('F', code) : Configuration.defaultFeedrate; // Ver qué valor va cuando no hay valor - CONFIGURACION? -
UnitsPosition startPosition = this.CurrentPosition;
logger.Info("procesando curva XY");
UnitsPosition finalPosition = this.GetFinalPosition(code);
UnitsPosition centerPosition = this.GetCenterPosition(code);
int gCode = Convert.ToInt32(GetValueParameter('G', code));
bool clockwise = gCode == 2 ? true : false;
double aX, aY, bX, bY, angleA, angleB, angle, radius, length;
int steps;
aX = currentPosition.X - centerPosition.X;
aY = currentPosition.Y - centerPosition.Y;
bX = finalPosition.X - centerPosition.X;
bY = finalPosition.Y - centerPosition.Y;
angleA = clockwise ? Math.Atan2(bY, bX) : Math.Atan2(aY, aX);
angleB = clockwise ? Math.Atan2(aY, aX) : Math.Atan2(bY, bX);
if (angleB <= angleA) angleB += 2 * Math.PI;
angle = angleB - angleA;
radius = Math.Sqrt(aX * aX + aY * aY);
length = radius * angle;
steps = (int)(length / Configuration.millimetersCurveSection);
steps = steps == 0 ? 1 : steps;
for (int s = 1; s <= steps; s++)
{
UnitsPosition sectionPosition = new UnitsPosition();
int step = clockwise ? steps - s : s;
sectionPosition.X = centerPosition.X + radius * Math.Cos(angleA + angle * ((float)step / steps));
sectionPosition.Y = centerPosition.Y + radius * Math.Sin(angleA + angle * ((float)step / steps));
sectionPosition.Z = (finalPosition.Z - startPosition.Z) * ((float)s / steps) + this.ReferencePosition.Z;
if ((sectionPosition.X < 0) || (sectionPosition.Y < 0) || (sectionPosition.Z < 0))
{
logger.Error("Underflow procesando: " + code + ".");
throw new Exception("Underflow procesando: " + code + ". (PuntoDestino: X" + sectionPosition.X + " Y" + sectionPosition.Y + " Z" + sectionPosition.Z);
}
// Traducir la curva (G02 / G03) como varias rectas (G01)
result.Add(sectionPosition);
}
return result;
}
protected CommandPreprocessor()
{
this.workingPlane = WorkingPlane.XY;
this.currentPosition = new UnitsPosition(0, 0, 0);
this.referencePosition = new UnitsPosition(0, 0, 0);
}
private UnitsPosition GetFromRadius(double r, string command)
{
double midX, midY, midZ, cat1x, cat1y, cat1z, cat1, cat2x, cat2y, cat2z, cat2;
logger.Info("Get from radius");
UnitsPosition final = GetFinalPosition(command);
UnitsPosition centerPosition = new UnitsPosition();
// componentes de Punto Medio de segmento que une Punto Inicial con Punto Final
midZ = (final.Z + CurrentPosition.Z) / 2;
midY = (final.Y + CurrentPosition.Y) / 2;
midX = (final.X + CurrentPosition.X) / 2;
// componentes de Longitud desde Punto Inicial a Punto Medio de segmento
cat1x = midX - CurrentPosition.X;
cat1y = midY - CurrentPosition.Y;
cat1z = midZ - CurrentPosition.Z;
switch (this.WorkingPlane)
{
case WorkingPlane.XY:
cat1 = Math.Sqrt(cat1x * cat1x + cat1y * cat1y);
if (cat1 > Math.Abs(r))
{
logger.Error("Imposible realizar arco: " + command + ". (Distancia media entre punto inicial y final es mayor al radio)");
throw new Exception("Imposible realizar arco: " + command + ". (Distancia media entre punto inicial y final es mayor al radio)");
}
cat2 = Math.Sqrt(r * r - cat1 * cat1);
cat2x = cat1y * cat2 / cat1;
cat2y = -cat1x * cat2 / cat1;
if (r > 0)
{
centerPosition.X = midX + cat2x;
centerPosition.Y = midY + cat2y;
}
else
{
centerPosition.X = midX - cat2x;
centerPosition.Y = midY - cat2y;
}
centerPosition.Z = midZ;
break;
case WorkingPlane.XZ:
cat1 = Math.Sqrt(cat1x * cat1x + cat1z * cat1z);
if (cat1 > Math.Abs(r))
{
logger.Error("Imposible realizar arco: " + command + ". (Distancia media entre punto inicial y final es mayor al radio)");
throw new Exception("Imposible realizar arco: " + command + ". (Distancia media entre punto inicial y final es mayor al radio)");
}
cat2 = Math.Sqrt(r * r - cat1 * cat1);
cat2x = cat1z * cat2 / cat1;
cat2z = -cat1x * cat2 / cat1;
if (r > 0)
{
centerPosition.X = midX + cat2x;
centerPosition.Z = midZ + cat2z;
}
else
{
centerPosition.X = midX - cat2x;
centerPosition.Z = midZ - cat2z;
}
centerPosition.Y = midY;
break;
case WorkingPlane.YZ:
cat1 = Math.Sqrt(cat1y * cat1y + cat1z * cat1z);
if (cat1 > Math.Abs(r))
{
logger.Error("Imposible realizar arco: " + command + ". (Distancia media entre punto inicial y final es mayor al radio)");
throw new Exception("Imposible realizar arco: " + command + ". (Distancia media entre punto inicial y final es mayor al radio)");
}
cat2 = Math.Sqrt(r * r - cat1 * cat1);
cat2y = cat1z * cat2 / cat1;
cat2z = -cat1y * cat2 / cat1;
if (r > 0)
{
centerPosition.Y = midY + cat2y;
centerPosition.Z = midZ + cat2z;
}
else
{
centerPosition.Y = midY - cat2y;
centerPosition.Z = midZ - cat2z;
}
centerPosition.X = midX;
break;
}
return centerPosition;
}
private UnitsPosition GetFinalPosition(string command)
{
UnitsPosition result = new UnitsPosition();
if (Configuration.absoluteProgamming)
{
result.X = HasValueParameter('X', command) ? GetValueParameter('X', command) + this.ReferencePosition.X : CurrentPosition.X;
result.Y = HasValueParameter('Y', command) ? GetValueParameter('Y', command) + this.ReferencePosition.Y : CurrentPosition.Y;
result.Z = HasValueParameter('Z', command) ? this.ReferencePosition.Z - GetValueParameter('Z', command) : CurrentPosition.Z;
}
else
{
// Translate to relative programming mode
result.X = HasValueParameter('X', command) ? GetValueParameter('X', command) + this.CurrentPosition.X : CurrentPosition.X;
result.Y = HasValueParameter('Y', command) ? GetValueParameter('Y', command) + this.CurrentPosition.Y : CurrentPosition.Y;
result.Z = HasValueParameter('Z', command) ? this.CurrentPosition.Z - GetValueParameter('Z', command) : CurrentPosition.Z;
}
logger.Info("voy a ir al Z " + result.Z);
if (result.Z < 0) throw new Exception("Movimiento Inválido. (Eje Z sobrepasa límite superior)");
// If the values are in inches, convert to millimeters
if (!Configuration.millimetersProgramming)
{
result.X *= 25.4;
result.Y *= 25.4;
result.Z *= 25.4;
}
return result;
}