private void UpdateOutput()
{
lock (updateLock)
{
try
{
var list = new List<PointError>();
foreach (Match match in ParsingRegex.Matches(inputTextBox.Text))
{
list.Add(new PointError(double.Parse(match.Groups["x"].Value), double.Parse(match.Groups["y"].Value), SignMultiplier * double.Parse(match.Groups["z"].Value)));
}
double center;
if (list.Any(_ => _.X == 0 && _.Y == 0))
{
center = list.First(_ => _.X == 0 && _.Y == 0).ZError;
}
else
{
var nearest =
(
from l in list
let dist = Math.Sqrt(Math.Pow(l.X, 2) + Math.Pow(l.Y, 2))
orderby dist
select new { l, dist }
)
.Take(4);
var totalDist = nearest.Sum(_ => _.dist);
center = nearest.Sum(_ => _.l.ZError * (_.dist / totalDist));
}
if (radioZCorrection.Checked)
{
var sb = new StringBuilder();
var cleanList = list.Select(_ => new PointError(_.X, _.Y, _.ZError - center)).ToList();
var lines =
from item in cleanList
select String.Format(@"G33 X{0:0.00} Y{1:0.00} Z{2:0.000}", item.X, item.Y, item.ZError);
sb.AppendLine("G33 R0");
outputTextBox.Text = "G33 R0\r\n" + String.Join("\r\n", lines);
}
else
{
outputTextBox.Text = String.Empty;
}
double stepsPerMm = double.Parse(txtStepsPerMmInput.Text);
if (stepsPerMm == 0)
throw new Exception("Invalid steps per mm");
var delta = new Delta(
diagonal: double.Parse(txtArmLengthInput.Text),
radius: double.Parse(txtRadiusInput.Text),
height: double.Parse(txtHeightInput.Text),
xstop: double.Parse(txtXEndstopInput.Text) / stepsPerMm,
ystop: double.Parse(txtYEndstopInput.Text) / stepsPerMm,
zstop: double.Parse(txtZEndstopInput.Text) / stepsPerMm,
xadj: double.Parse(txtXadjustInput.Text),
yadj: double.Parse(txtYAdjustInput.Text),
zadj: double.Parse(txtZAdjustInput.Text)
);
double bedRadius = double.Parse(txtBedRadiusInput.Text);
double bedRadiusSquared = bedRadius * bedRadius;
//var invertSigns =
// cleanList
// .Select(_ => new PointError(_.X, _.Y, -1 * _.ZError))
// .ToList();
//var result = delta.DoDeltaCalibration(7, invertSigns.ToList(), true);
var result = delta.DoDeltaCalibration(7, list.ToList(), true);
txtInitialError.Text = String.Format("{0:0.00}", result.Item1);
txtAdjustedError.Text = String.Format("{0:0.00}", result.Item2);
txtArmLengthOutput.Text = String.Format("{0:0.00}", delta.diagonal);
txtHeightOutput.Text = String.Format("{0:0.00}", delta.homedHeight);
txtRadiusOutput.Text = String.Format("{0:0.00}", delta.radius);
txtXadjustOutput.Text = String.Format("{0:0.00}", delta.xadj);
txtYAdjustOutput.Text = String.Format("{0:0.00}", delta.yadj);
txtZAdjustOutput.Text = String.Format("{0:0.00}", delta.zadj);
txtXEndstopOutput.Text = String.Format("{0:0.00}", delta.xstop * stepsPerMm);
txtYEndstopOutput.Text = String.Format("{0:0.00}", delta.ystop * stepsPerMm);
txtZEndstopOutput.Text = String.Format("{0:0.00}", delta.zstop * stepsPerMm);
}
catch (Exception)
{
txtArmLengthOutput.Text
= txtHeightOutput.Text
= txtRadiusOutput.Text
= txtXadjustOutput.Text
= txtYAdjustOutput.Text
= txtZAdjustOutput.Text
= txtXEndstopOutput.Text
= txtYEndstopOutput.Text
= txtZEndstopOutput.Text
= String.Empty;
}
}
}
private double ComputeDerivative(int deriv, double ha, double hb, double hc)
{
var perturb = 0.2; // perturbation amount in mm or degrees
var hiParams = new Delta(this.diagonal, this.radius, this.homedHeight, this.xstop, this.ystop, this.zstop, this.xadj, this.yadj, this.zadj);
var loParams = new Delta(this.diagonal, this.radius, this.homedHeight, this.xstop, this.ystop, this.zstop, this.xadj, this.yadj, this.zadj);
switch (deriv)
{
case 0:
case 1:
case 2:
break;
case 3:
hiParams.radius += perturb;
loParams.radius -= perturb;
break;
case 4:
hiParams.xadj += perturb;
loParams.xadj -= perturb;
break;
case 5:
hiParams.yadj += perturb;
loParams.yadj -= perturb;
break;
case 6:
hiParams.diagonal += perturb;
loParams.diagonal -= perturb;
break;
}
hiParams.Recalc();
loParams.Recalc();
var zHi = hiParams.InverseTransform((deriv == 0) ? ha + perturb : ha, (deriv == 1) ? hb + perturb : hb, (deriv == 2) ? hc + perturb : hc);
var zLo = loParams.InverseTransform((deriv == 0) ? ha - perturb : ha, (deriv == 1) ? hb - perturb : hb, (deriv == 2) ? hc - perturb : hc);
return (zHi - zLo) / (2 * perturb);
}