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); } } }