private MotionProfile.Path CreateNewPath()
{
MotionProfile.Path path = new MotionProfile.Path();
path.velocityMap = new MotionProfile.VelocityMap();
path.velocityMap.vMax = int.Parse(maxVelocity.Text);
path.velocityMap.FL1 = int.Parse(AccelRate.Text);
path.velocityMap.time = float.Parse(timeSample.Text) / 1000;
path.tolerence = float.Parse(tolerence.Text);
// path.mindifference = float.Parse(Calibration.Text);
path.velocityMap.instVelocity = isntaVel.Checked;
path.speedLimit = float.Parse(SpeedLimit.Text);
path.calibration = TurnCheck.Checked;
return(path);
}
private void Apply_Click(object sender, EventArgs e)
{
MotionProfile.Path path = CreateNewPath();
int trackwidth = (int)((int.Parse(trackWidth.Text)) / 2);
ClearChart(mainField);
paths = new MotionProfile.Trajectory();
string last = "";
DataGridViewRow lastrow = controlPoints.Rows[0];
foreach (DataGridViewRow row in controlPoints.Rows)
{
if (row.Cells[0].Value != null)
{
lastrow = row;
mainField.Series["cp"].Points.AddXY(float.Parse(row.Cells[0].Value.ToString()), float.Parse(row.Cells[1].Value.ToString()));
if (path.controlPoints.Count == 0)
{
if (row.Cells[2].Value.ToString() == "-")
{
path.direction = true;
}
if (row.Cells[2].Value.ToString() == "+")
{
path.direction = false;
}
}
if (row.Cells[2].Value.ToString() == "-")
{
mainField.Series["cp"].Points.Last().Color = Color.Red;
path.direction = true;
}
if (row.Cells[2].Value.ToString() == "+")
{
path.direction = false;
}
path.addControlPoint(float.Parse(row.Cells[1].Value.ToString()), float.Parse(row.Cells[0].Value.ToString()));
if (last != "" && last != row.Cells[2].Value.ToString())
{
if (row.Cells[2].Value.ToString() == "+")
{
path.direction = false;
}
if (row.Cells[2].Value.ToString() == "-")
{
path.direction = true;
}
if (path.controlPoints.Count >= 2)
{
paths.Add(path);
}
path = CreateNewPath();
path.velocityMap.instVelocity = isntaVel.Checked;
path.addControlPoint(float.Parse(row.Cells[1].Value.ToString()), float.Parse(row.Cells[0].Value.ToString()));
}
last = row.Cells[2].Value.ToString();
}
}
if (path.controlPoints.Count() == 0)
{
return;
}
if (lastrow != null && lastrow.Cells[2].Value.ToString() != "+")
{
path.direction = false;
}
if (lastrow != null && lastrow.Cells[2].Value.ToString() != "-")
{
path.direction = true;
}
if (path.controlPoints.Count >= 2)
{
paths.Add(path);
}
if (!checkBox1.Checked)
{
paths.test();
}
else
{
paths.Create(0);
}
ClearChart(VelocityPlot);
ClearChart(DistancePlot);
float[] t, d, v, l, r, ld, rd, c, cd;
if (TurnCheck.Checked)
{
if (int.Parse(degrees.Text) > 0)
{
foreach (MotionProfile.Path p in paths)
{
p.direction = !p.direction;
}
}
t = paths.getTimeProfile();
d = paths.getDistanceProfile();
v = paths.getVelocityProfile();
l = paths.getOffsetVelocityProfile(trackwidth).ToArray();
ld = paths.getOffsetDistanceProfile(trackwidth).ToArray();
c = paths.getOffsetVelocityProfile(0).ToArray();
cd = paths.getOffsetDistanceProfile(0).ToArray();
l.NoiseReduction(int.Parse(smoothness.Text));
if (int.Parse(degrees.Text) > 0)
{
foreach (MotionProfile.Path p in paths)
{
p.direction = !p.direction;
}
}
if (int.Parse(degrees.Text) < 0)
{
foreach (MotionProfile.Path p in paths)
{
p.direction = !p.direction;
}
}
r = paths.getOffsetVelocityProfile(-trackwidth).ToArray();
rd = paths.getOffsetDistanceProfile(-trackwidth).ToArray();
if (int.Parse(degrees.Text) < 0)
{
foreach (MotionProfile.Path p in paths)
{
p.direction = !p.direction;
}
}
}
else
{
t = paths.getTimeProfile();
d = paths.getDistanceProfile();
v = paths.getVelocityProfile();
l = paths.getOffsetVelocityProfile(trackwidth).ToArray();
ld = paths.getOffsetDistanceProfile(trackwidth).ToArray();
c = paths.getOffsetVelocityProfile(0).ToArray();
cd = paths.getOffsetDistanceProfile(0).ToArray();
l.NoiseReduction(int.Parse(smoothness.Text));
r = paths.getOffsetVelocityProfile(-trackwidth).ToArray();
rd = paths.getOffsetDistanceProfile(-trackwidth).ToArray();
}
r.NoiseReduction(int.Parse(smoothness.Text));
rd.NoiseReduction(int.Parse(smoothness.Text));
l.NoiseReduction(int.Parse(smoothness.Text));
rd.NoiseReduction(int.Parse(smoothness.Text));
c.NoiseReduction(int.Parse(smoothness.Text));
cd.NoiseReduction(int.Parse(smoothness.Text));
double ldv = 0;
double rdv = 0;
for (int i = 0; i < ld.Length; i++)
{
ldv += ld[i];
rdv += rd[i];
DistancePlot.Series["left"].Points.AddXY(t[i], ldv);
DistancePlot.Series["right"].Points.AddXY(t[i], rdv);
}
for (int i = 0; i < Math.Min(d.Length, r.Length); i++)
{
VelocityPlot.Series["path"].Points.AddXY(d[i], v[i + 2]);
VelocityPlot.Series["left"].Points.AddXY(d[i], l[i]);
VelocityPlot.Series["right"].Points.AddXY(d[i], r[i]);
}
mainField.Series["path"].Points.Clear();
mainField.Series["left"].Points.Clear();
mainField.Series["right"].Points.Clear();
TEMPXLIST.Clear();
TEMPYLIST.Clear();
TEMPforwardLIST.Clear();
foreach (Point p in paths.BuildPath())
{
foreach (PointF p1 in p.point)
{
mainField.Series["path"].Points.AddXY(p1.Y, p1.X);
TEMPXLIST.Add(p1.X);
TEMPYLIST.Add(p1.Y);
TEMPforwardLIST.Add(p.direction);
}
}
foreach (Point p in paths.BuildPath(trackwidth))
{
foreach (PointF p1 in p.point)
{
mainField.Series["left"].Points.AddXY(p1.Y, p1.X);
}
}
foreach (Point p in paths.BuildPath(-trackwidth))
{
foreach (PointF p1 in p.point)
{
mainField.Series["right"].Points.AddXY(p1.Y, p1.X);
}
}
}
