public void Reset()
{
PosicionLinea = 0;
this.PosicionInicial = new Vector5D(0.0m, 0.0m, 0.0m, 0.0m, 0.0m);
this.PosicionFinal = new Vector5D(0.0m, 0.0m, 0.0m, 0.0m, 0.0m);
this.Delta = new Vector5D(0.0m, 0.0m, 0.0m, 0.0m, 0.0m);
}
/*public static Vector5D Abs(Vector5D v)
{
v.X = Math.Abs(v.X);
v.Y = Math.Abs(v.Y);
v.Z = Math.Abs(v.Z);
v.E = Math.Abs(v.E);
v.F = Math.Abs(v.F);
return v;
}*/
public static Vector5D Abs(Vector5D v)
{
return new Vector5D(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z), Math.Abs(v.E), Math.Abs(v.F));
}
private void SetPosition(string args)
{
ProcesarCampoNumerico(args, "X", ref this.PosicionFinal.X);
ProcesarCampoNumerico(args, "Y", ref this.PosicionFinal.Y);
ProcesarCampoNumerico(args, "Z", ref this.PosicionFinal.Z);
ProcesarCampoNumerico(args, "E", ref this.PosicionFinal.E);
ProcesarCampoNumerico(args, "F", ref this.PosicionFinal.F);
this.PosicionInicial = this.PosicionFinal;
}
// 0.5, porque OSA toma PeriodoPulsos como semiPeriodoPulsos
private void PromediarPeriodo(ref Vector5D T, Vector5D d, decimal f)
{
if ((d.E == 0) && (d.Y == 0) && (d.X == 0))
{
T.E = T.Y = T.X = 0;
}
if ((d.E == 0) && (d.Y == 0) && (d.X != 0))
{
T.X = decimal.Round(3.3m * 7m * f * 0.5m);
}
if ((d.E == 0) && (d.Y != 0) && (d.X == 0))
{
T.Y = decimal.Round(7m * f * 0.5m);
}
if ((d.E == 0) && (d.Y != 0) && (d.X != 0))
{
T.Y = decimal.Round(7m * f * 0.5m);
T.X = decimal.Round((d.Y / d.X) * T.Y);
}
if ((d.E != 0) && (d.Y == 0) && (d.X == 0))
{
T.E = decimal.Round(f * 0.5m);
}
if ((d.E != 0) && (d.Y == 0) && (d.X != 0))
{
T.E = decimal.Round(f * 0.5m);
T.X = decimal.Round((d.E / d.X) * f * 0.5m);
}
if ((d.E != 0) && (d.Y != 0) && (d.X == 0))
{
T.E = decimal.Round(f * 0.5m);
T.Y = decimal.Round((d.E / d.Y) * f * 0.5m);
}
if ((d.E != 0) && (d.Y != 0) && (d.X != 0))
{
T.E = decimal.Round(f * 0.5m);
T.Y = decimal.Round((d.E / d.Y) * f * 0.5m);
T.X = decimal.Round((d.E / d.X) * f * 0.5m);
}
if (d.Z != 0) T.Z = decimal.Round(7m * f * 0.5m);
if (d.Z == 0) T.Z = 0;
}
private void Escalar(ref Vector5D Fin, Vector5D Ini)
{
if (Fin.X != Ini.X) Fin.X = decimal.Round(Fin.X * 3.03m); //1[mm] = 3.03[pulsos]
if (Fin.Y != Ini.Y) Fin.Y = decimal.Round(Fin.Y * 10m); //1[mm] = 10[pulsos]
if (Fin.Z != Ini.Z) Fin.Z = decimal.Round(Fin.Z * 50m); //1[mm] = 50[pulsos]
if (Fin.E != Ini.E) Fin.E = decimal.Round(Fin.E * 70m); //1[mm] = 70[pulsos]
if (Fin.F != Ini.F) Fin.F = decimal.Round(2m * Convert.ToDecimal(Math.Pow(2, Convert.ToDouble(-Fin.F * 2m / (60m * 20m)))) * 20m); //10[mm/s] = 20[ms]
}
private void DecidirDirMotor(ref Vector5D dir, Vector5D Fin, Vector5D Ini)
{
if (Fin.X > Ini.X) dir.X = 128;
if (Fin.X < Ini.X) dir.X = 64;
if (Fin.Y > Ini.Y) dir.Y = 32;
if (Fin.Y < Ini.Y) dir.Y = 16;
if (Fin.Z > Ini.Z) dir.Z = 8;
if (Fin.Z < Ini.Z) dir.Z = 4;
if (Fin.E > Ini.E) dir.E = 2;
if (Fin.E < Ini.E) dir.E = 1;
}
private void ControlledMove(string args)
{
ProcesarCampoNumerico(args, "X", ref this.PosicionFinal.X);
ProcesarCampoNumerico(args, "Y", ref this.PosicionFinal.Y);
ProcesarCampoNumerico(args, "Z", ref this.PosicionFinal.Z);
ProcesarCampoNumerico(args, "E", ref this.PosicionFinal.E);
ProcesarCampoNumerico(args, "F", ref this.PosicionFinal.F);
Escalar(ref this.PosicionFinal, this.PosicionInicial);
this.Delta = Vector5D.Abs(this.PosicionFinal - this.PosicionInicial);
PromediarPeriodo(ref this.Periodo, this.Delta, this.PosicionFinal.F);
DecidirDirMotor(ref this.DirMotor, this.PosicionFinal, this.PosicionInicial);
this.PosicionInicial = this.PosicionFinal;
}