private void 路径绘制_Click(object sender, EventArgs e) { // if (MouseDownMode == "Bspline绘制") { if (DrawPointFlag == "未填入") { MessageBox.Show("请输入姿态(单位:度)", "注意", MessageBoxButtons.OK, MessageBoxIcon.Warning); return; } //完成绘制结束工作 MouseDownMode = ""; 路径绘制.Text = "路径绘制"; DRAW_INF = "绘制完成"; KeyPointNumLabel.Text = ""; //单独一个点是不允许的 if (keyPoints.Count() == 1) { return; } StreamWriter keyPointFile = new StreamWriter(BsplineDrawPathInf, false); Graphics myGra = viewControl.CreateGraphics(); Pen myPen = new Pen(System.Drawing.Color.Red, 2); foreach (KeyPoint keyP in keyPoints) { keyPointFile.WriteLine("{0:f2},{1:f2},{2:f2}", keyP.point.X, keyP.point.Y, keyP.direction); } keyPointFile.Close(); //进行分段处理,便于速度规划和数据传输 Bspline.BsplineSegment(BsplineDrawPathInf, PathAllInf); //曲率半径计算 SpeedPlanning.CalculateCurvature(); PathClear = "绘制路径"; 路径恢复.Enabled = true; 路径清空.Enabled = true; viewControl.Refresh(); } else { //开始绘制准备工作 MessageBox.Show("默认第一点为(0,0),姿态为0度", "注意", MessageBoxButtons.OK, MessageBoxIcon.Warning); keyPoints.Clear(); keyPoints.Add(new KeyPoint(0.0f, 0.0f, 0.0f)); KeyPointNumLabel.Text = keyPoints.Count.ToString(); MouseDownMode = "Bspline绘制"; 路径绘制.Text = "完成绘制"; DRAW_INF = "正在绘制"; 路径恢复.Enabled = false; 路径清空.Enabled = false; } }
private void SpeedReplanning() { //将txt文档中的数据存入ringBuffer SpeedPlanning.MoveTxtToRingBuffer(); int num = PointsInfo.pnts.Count(); float[] angleErr = new float[num]; //数据读取 StreamReader speedReplanFile = File.OpenText(SpeedReplanInfo); List <string> tempString = new List <string>(); while (!speedReplanFile.EndOfStream) { tempString.Add(speedReplanFile.ReadLine()); } speedReplanFile.Close(); if (num < 3) { return; } //计算每一段的平均角度偏差 //将一段里面的打滑量放在后端点数组 for (int i = 1; i < num; i++) { float angleErrSum = 0; int count = 0; foreach (string str in tempString) { string[] sArray = str.Split(','); if (float.Parse(sArray[0]) > PointsInfo.pnts[i - 1].length && float.Parse(sArray[0]) < PointsInfo.pnts[i].length) { angleErrSum += float.Parse(sArray[1]); count++; } if (float.Parse(sArray[0]) > PointsInfo.pnts[i].length) { break; } } if (count != 0) { angleErr[i] = angleErrSum / count; } else { angleErr[i] = 0; } } //对第一点和第二点进行忽略 for (int i = 1; i < num - 1; i++) { if (angleErr[i] > 0.1f) { if (angleErr[i] > 40) { angleErr[i] = 40; } PointsInfo.pnts[i - 1].velMax *= (1 - angleErr[i] / 100); } } float tempVell = 0.0f; //通过v2^2 - v1^2 = 2*a*s对速度再次规划 for (int i = 0; i < num - 1; i++) { if (PointsInfo.pnts[i + 1].velMax > PointsInfo.pnts[i].velMax) { tempVell = (float)Math.Sqrt(2 * (MotionCardParameter.GetAccMax()) * (PointsInfo.pnts[i + 1].length - PointsInfo.pnts[i].length) + PointsInfo.pnts[i].velMax * PointsInfo.pnts[i].velMax); if (tempVell < PointsInfo.pnts[i + 1].velMax) { PointsInfo.pnts[i + 1].velMax = tempVell; } } } for (int i = num - 1; i > 0; i--) { if (PointsInfo.pnts[i - 1].velMax > PointsInfo.pnts[i].velMax) { tempVell = (float)Math.Sqrt(2 * (MotionCardParameter.GetAccMax()) * (PointsInfo.pnts[i].length - PointsInfo.pnts[i - 1].length) + PointsInfo.pnts[i].velMax * PointsInfo.pnts[i].velMax); if (tempVell < PointsInfo.pnts[i - 1].velMax) { PointsInfo.pnts[i - 1].velMax = tempVell; } } } for (int i = 0; i < num; i++) { Console.WriteLine(PointsInfo.pnts[i].velMax); } SpeedPlanning.MoveListToTxt(); //将速度小于最小速度的做处理 for (int i = 0; i < num; i++) { if (PointsInfo.pnts[i].velMax < 70) { PointsInfo.pnts[i].velMax = 70; } } }