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