private void VelDisplay_Paint(object sender, PaintEventArgs e)
{
//this.AutoScrollMinSize = new Size(ClientRectangle.Width, ClientRectangle.Height);
//绘制虚线表格
Graphics myGra = VelDisplay.CreateGraphics();
Pen myPen = new Pen(Color.Black, 1);
myPen.EndCap = LineCap.ArrowAnchor;
//绘画栅格
ControlPaint.DrawGrid(e.Graphics, this.ClientRectangle, new Size(20, 20), Color.White);
//绘制坐标系中心
AdjustableArrowCap myLineCap = new AdjustableArrowCap(3, 3, true);
myPen.CustomEndCap = myLineCap;
myGra.DrawLine(myPen, 10, VelDisplay.Bottom, 10, 5);
myGra.DrawLine(myPen, 0, VelDisplay.Bottom / 2, VelDisplay.Right - 10, VelDisplay.Bottom / 2);
switch (VelDisplayBox.Text)
{
case "1号轮速度":
case "2号轮速度":
case "3号轮速度":
case "全部速度":
WheelVelCurveDrawing();
单位.Text = "mm/s";
break;
case "合速度":
case "合速度方向":
case "旋转速度":
ResultantVelCurveDrawing();
break;
case "位置误差":
ErrCurveDrawing();
ResultantVelCurveDrawing();
单位.Text = "mm";
break;
default:
break;
}
//ErrCurveDrawing();
}
private void ErrCurveDrawing()
{
//数据读取
StreamReader pathFile = File.OpenText(VelInf);
List <string> tempString = new List <string>();
while (!pathFile.EndOfStream)
{
tempString.Add(pathFile.ReadLine());
}
pathFile.Close();
//如果点数小于2，不能规划出轨迹
if (tempString.Count() < 2)
{
return;
}
Graphics myGra = VelDisplay.CreateGraphics();
Pen myPen = new Pen(Color.Orange, 1);
PointF[] Err = new PointF[tempString.Count()];
int i = 0;
foreach (string str in tempString)
{
string[] sArray = str.Split(',');
Err[i].X = float.Parse(sArray[0]);
Err[i].Y = float.Parse(sArray[7]);
i++;
}
pathMax = FindMaxAbs(Err).X;
float iii = FindMaxAbs(Err).Y;
zoom = (float)(VelDisplay.Bottom / 2) / FindMaxAbs(Err).Y;
//设置滚动条最大值
hScrollBar1.Maximum = (int)(PERCENT * pathMax / (VelDisplay.Right / zoomL)) + 1;
for (int j = 0; j < Err.Count(); j++)
{
Err[j] = GetPanelAxes(Err[j].X, Err[j].Y);
}
for (int m = 0; m < 4; m++)
{
for (int j = 1; j < Err.Count() - 1; j++)
{
Err[j].Y = (Err[j - 1].Y + Err[j + 1].Y) / 2;
}
}
myGra.DrawLines(myPen, Err);
}
//绘制合速度图像
void ResultantVelCurveDrawing()
{
//数据读取
StreamReader pathFile = File.OpenText(VelInf);
List <string> tempString = new List <string>();
while (!pathFile.EndOfStream)
{
tempString.Add(pathFile.ReadLine());
}
pathFile.Close();
//如果点数小于2，不能规划出轨迹
if (tempString.Count() < 2)
{
return;
}
Graphics myGra = VelDisplay.CreateGraphics();
Pen myPen = new Pen(Color.Red, 1);
PointF[] velWheel1 = new PointF[tempString.Count()];
PointF[] velWheel2 = new PointF[tempString.Count()];
PointF[] velWheel3 = new PointF[tempString.Count()];
PointF[] targetVelWheel1 = new PointF[tempString.Count()];
PointF[] targetVelWheel2 = new PointF[tempString.Count()];
PointF[] targetVelWheel3 = new PointF[tempString.Count()];
float[] zAngle = new float[tempString.Count()];
int i = 0;
foreach (string str in tempString)
{
string[] sArray = str.Split(',');
velWheel1[i].X = velWheel2[i].X = velWheel3[i].X
= targetVelWheel1[i].X = targetVelWheel2[i].X = targetVelWheel3[i].X
= float.Parse(sArray[0]);
velWheel1[i].Y = float.Parse(sArray[1]);
velWheel2[i].Y = float.Parse(sArray[2]);
velWheel3[i].Y = float.Parse(sArray[3]);
targetVelWheel1[i].Y = float.Parse(sArray[4]);
targetVelWheel2[i].Y = float.Parse(sArray[5]);
targetVelWheel3[i].Y = float.Parse(sArray[6]);
i++;
}
PointF[] trueVel = new PointF[tempString.Count()];
PointF[] targetVel = new PointF[tempString.Count()];
PointF[] trueDir = new PointF[tempString.Count()];
PointF[] targetDir = new PointF[tempString.Count()];
PointF[] trueRotateVel = new PointF[tempString.Count()];
PointF[] targetRotateVel = new PointF[tempString.Count()];
PointF[] DirectionErr = new PointF[tempString.Count()];
PointF[] binaryzation = new PointF[tempString.Count()];
//由三个轮子的速度计算合成速度
for (int j = 0; j < targetVelWheel1.Count(); j++)
{
Calculate.TriWheelVel2_t tempVel;
tempVel = Calculate.TriWheelVel2ResultantVel(velWheel1[j].Y, velWheel2[j].Y, velWheel3[j].Y, zAngle[j]);
trueVel[j].Y = tempVel.speed;
trueVel[j].X = velWheel1[j].X;
trueDir[j].Y = tempVel.direction;
trueDir[j].X = velWheel1[j].X;
trueRotateVel[j].Y = tempVel.rotationVel;
trueRotateVel[j].X = velWheel1[j].X;
tempVel = Calculate.TriWheelVel2ResultantVel(targetVelWheel1[j].Y, targetVelWheel2[j].Y, targetVelWheel3[j].Y, zAngle[j]);
targetVel[j].Y = tempVel.speed;
targetVel[j].X = targetVelWheel1[j].X;
targetDir[j].Y = tempVel.direction;
targetDir[j].X = targetVelWheel1[j].X;
targetRotateVel[j].Y = tempVel.rotationVel;
targetRotateVel[j].X = targetVelWheel1[j].X;
}
pathMax = FindMaxAbs(targetDir).X;
zoom = (float)(VelDisplay.Bottom / 2) / FindMaxAbs(targetDir).Y;
//设置滚动条最大值
hScrollBar1.Maximum = (int)(PERCENT * pathMax / (VelDisplay.Right / zoomL)) + 1;
for (int j = 0; j < targetVelWheel1.Count(); j++)
{
DirectionErr[j].X = targetDir[j].X;
DirectionErr[j].Y = Math.Abs(targetDir[j].Y - trueDir[j].Y);
if (DirectionErr[j].Y > 180)
{
DirectionErr[j].Y = 360 - DirectionErr[j].Y;
}
else if (DirectionErr[j].Y < -180)
{
DirectionErr[j].Y = 360 + DirectionErr[j].Y;
}
}
//进行滤波处理
for (int m = 0; m < 4; m++)
{
for (int j = 1; j < DirectionErr.Count() - 1; j++)
{
DirectionErr[j].Y = (DirectionErr[j - 1].Y + DirectionErr[j + 1].Y) / 2;
}
}
//进行阈值判断，二值化
for (int j = 0; j < DirectionErr.Count(); j++)
{
if (DirectionErr[j].Y < 8)
{
binaryzation[j].Y = 0;
binaryzation[j].X = DirectionErr[j].X;
}
else
{
binaryzation[j].Y = DirectionErr[j].Y;
binaryzation[j].X = DirectionErr[j].X;
}
}
//将二值化的文件存入txt文档中
StreamWriter SpeedFile = new StreamWriter(SpeedReplanInfo, false);
for (int m = 0; m < binaryzation.Count(); m++)
{
SpeedFile.WriteLine("{0:f0},{1:f0}", binaryzation[m].X, binaryzation[m].Y);
}
SpeedFile.Close();
//进行阈值判断，二值化
for (int j = 0; j < DirectionErr.Count(); j++)
{
binaryzation[j] = GetPanelAxes(binaryzation[j].X, binaryzation[j].Y);
}
myPen.Color = Color.Black;
myGra.DrawLines(myPen, binaryzation);
}
//绘画三个轮速度图像
private void WheelVelCurveDrawing()
{
//数据读取
StreamReader pathFile = File.OpenText(VelInf);
List <string> tempString = new List <string>();
while (!pathFile.EndOfStream)
{
tempString.Add(pathFile.ReadLine());
}
pathFile.Close();
//如果点数小于2，不能规划出轨迹
if (tempString.Count() < 2)
{
return;
}
Graphics myGra = VelDisplay.CreateGraphics();
Pen myPen = new Pen(Color.Red, 1);
PointF[] velWheel1 = new PointF[tempString.Count()];
PointF[] velWheel2 = new PointF[tempString.Count()];
PointF[] velWheel3 = new PointF[tempString.Count()];
PointF[] targetVelWheel1 = new PointF[tempString.Count()];
PointF[] targetVelWheel2 = new PointF[tempString.Count()];
PointF[] targetVelWheel3 = new PointF[tempString.Count()];
int i = 0;
//设置zoom值
foreach (string str in tempString)
{
string[] sArray = str.Split(',');
velWheel1[i].X = velWheel2[i].X = velWheel3[i].X = float.Parse(sArray[0]);
velWheel1[i].Y = float.Parse(sArray[4]);
velWheel2[i].Y = float.Parse(sArray[5]);
velWheel3[i].Y = float.Parse(sArray[6]);
i++;
}
//计算速度最大值，用于调整画面比例
float[] vel = new float[6];
vel[0] = FindMaxAbs(velWheel1).Y;
vel[1] = FindMaxAbs(velWheel2).Y;
vel[2] = FindMaxAbs(velWheel3).Y;
zoom = (float)(VelDisplay.Bottom / 2) / vel.Max();
pathMax = FindMaxAbs(velWheel1).X;
//设置滚动条最大值
hScrollBar1.Maximum = (int)(PERCENT * pathMax / (VelDisplay.Right / zoomL)) + 1;
i = 0;
//读取文件
foreach (string str in tempString)
{
string[] sArray = str.Split(',');
velWheel1[i] = GetPanelAxes(float.Parse(sArray[0]), float.Parse(sArray[1]));
velWheel2[i] = GetPanelAxes(float.Parse(sArray[0]), float.Parse(sArray[2]));
velWheel3[i] = GetPanelAxes(float.Parse(sArray[0]), float.Parse(sArray[3]));
targetVelWheel1[i] = GetPanelAxes(float.Parse(sArray[0]), float.Parse(sArray[4]));
targetVelWheel2[i] = GetPanelAxes(float.Parse(sArray[0]), float.Parse(sArray[5]));
targetVelWheel3[i] = GetPanelAxes(float.Parse(sArray[0]), float.Parse(sArray[6]));
i++;
}
//进行滤波处理
for (int n = 0; n < 5; n++)
{
for (int j = 1; j < targetVelWheel1.Count() - 1; j++)
{
velWheel1[j].Y = (velWheel1[j - 1].Y + velWheel1[j + 1].Y) / 2;
velWheel2[j].Y = (velWheel2[j - 1].Y + velWheel2[j + 1].Y) / 2;
velWheel3[j].Y = (velWheel3[j - 1].Y + velWheel3[j + 1].Y) / 2;
targetVelWheel1[j].Y = (targetVelWheel1[j - 1].Y + targetVelWheel1[j + 1].Y) / 2;
targetVelWheel2[j].Y = (targetVelWheel2[j - 1].Y + targetVelWheel2[j + 1].Y) / 2;
targetVelWheel3[j].Y = (targetVelWheel3[j - 1].Y + targetVelWheel3[j + 1].Y) / 2;
}
}
switch (VelDisplayBox.Text)
{
case "1号轮速度":
myPen.Color = Color.Red;
myGra.DrawLines(myPen, velWheel1);
myPen.Color = Color.Blue;
myGra.DrawLines(myPen, targetVelWheel1);
break;
case "2号轮速度":
myPen.Color = Color.Red;
myGra.DrawLines(myPen, velWheel2);
myPen.Color = Color.Blue;
myGra.DrawLines(myPen, targetVelWheel2);
break;
case "3号轮速度":
myPen.Color = Color.Red;
myGra.DrawLines(myPen, velWheel3);
myPen.Color = Color.Blue;
myGra.DrawLines(myPen, targetVelWheel3);
break;
}
}