private Tuple <PointD, double> CaptrueDelta()
{
//拍照前延时
Thread.Sleep(this.settlingTime);
//拍照获取图像
byte[] imgData = Machine.Instance.Camera.TriggerAndGetBytes(TimeSpan.FromSeconds(1)).DeepClone();
//移动到拍照高度
if (Machine.Instance.Setting.MachineSelect == MachineSelection.RTV)
{
Machine.Instance.Robot.MoveToMarkZAndReply();
}
//抓圆，得到像素坐标
if (!this.chxAsv.Checked)
{
CalibMap.excuteCaliper(imgData, Machine.Instance.Camera.Executor.ImageWidth, Machine.Instance.Camera.Executor.ImageHeight,
ref outX, ref outY, ref outR);
}
else
{
this.inspection.Execute(imgData, Machine.Instance.Camera.Executor.ImageWidth, Machine.Instance.Camera.Executor.ImageHeight);
outX = this.inspection.PixResultX;
outY = this.inspection.PixResultY;
outR = this.inspection.PixResultR;
}
//像素坐标转机械坐标
double scale = 0;
PointD p = Machine.Instance.Camera.ToMachine(outX, outY);
CalibBy9d.GetScale(ref scale);
this.BeginInvoke(new MethodInvoker(() =>
{
this.LblTitle.Text = string.Format("deltaX = {0}, deltaY = {1}.", Math.Round(p.X, 3), Math.Round(p.Y, 3));
}));
return(new Tuple <PointD, double>(p, outR * scale));
}
private async void btnVerify_Click(object sender, EventArgs e)
{
if (this.flag == 6)
{
this.flag = 4;
}
PointD p00 = new PointD();
Machine.Instance.Robot.CalibMapPrm.VerifyItems.Clear();
this.listBox1.Items.Clear();
Random random = new Random();
//棋盘校正
this.stopping = false;
await Task.Factory.StartNew(() => {
Machine.Instance.Robot.MoveSafeZAndReply();
for (int i = 0; i < this.rowNum; i++)
{
for (int j = 0; j < this.colNum; j++)
{
if (this.stopping)
{
break;
}
Tuple <PointD, double> delta;
MapPoint mp = Machine.Instance.Robot.CalibMapPrm.Items.Find(m => m.IndexX == j && m.IndexY == i);
double dx = random.Next(-100, 100) * 0.01;
double dy = random.Next(-100, 100) * 0.01;
//dx = 0;
//dy = 0;
double machX = 0, machY = 0;
if (mp != null)
{
if (!this.chxRbfd.Checked)
{
CalibMap.mapToMach(mp.RobotX + dx, mp.RobotY + dy, ref machX, ref machY);
}
else
{
CalibNet.calcCoordinate(mp.RobotX + dx, mp.RobotY + dy, ref machX, ref machY);
}
if (i == 0 && j == 0)
{
p00.X = machX;
p00.Y = machY;
}
}
Machine.Instance.Robot.MovePosXYAndReply(machX, machY);
delta = this.CaptrueDelta();
MapPoint mp2 = new MapPoint()
{
RealX = -delta.Item1.X,
RealY = -delta.Item1.Y,
RobotX = dx,
RobotY = dy,
IndexX = j,
IndexY = i,
R = delta.Item2,
Ok = true
};
Machine.Instance.Robot.CalibMapPrm.VerifyItems.Add(mp2);
this.BeginInvoke(new MethodInvoker(() =>
{
this.listBox1.Items.Add(mp);
}));
//拍照后延时
Thread.Sleep(this.dwellTime);
}
}
Machine.Instance.Robot.MovePosXYAndReply(p00);
this.flag++;
this.BeginInvoke(new MethodInvoker(() =>
{
this.UpdateByFlag();
}));
});
this.flag++;
this.UpdateByFlag();
}
public async void DoNext()
{
switch (this.flag)
{
case 0:
//设置参数
this.origin = new PointD(tbOriginX.Value, tbOriginY.Value);
this.hend = new PointD(tbHendX.Value, tbHendY.Value);
this.vend = new PointD(tbVendX.Value, tbVendY.Value);
this.interval = (double)this.nudInterval.Value;
if (this.origin.DistanceTo(this.hend) < this.interval || this.origin.DistanceTo(this.vend) < this.interval)
{
//MessageBox.Show("Please teach right Xend and Yend.");
MessageBox.Show("请正确的示教结束点.");
return;
}
this.settlingTime = (int)this.nudSettlingTime.Value;
this.dwellTime = (int)this.nudDwellTime.Value;
this.rowNum = (int)(this.vend.DistanceTo(this.origin) / this.interval) + 1;
this.colNum = (int)(this.hend.DistanceTo(this.origin) / this.interval) + 1;
break;
case 1:
Machine.Instance.Robot.CalibMapPrm.RowNum = rowNum;
Machine.Instance.Robot.CalibMapPrm.ColNum = colNum;
Machine.Instance.Robot.CalibMapPrm.Interval = interval;
Machine.Instance.Robot.CalibMapPrm.Items.Clear();
Machine.Instance.Robot.IsMapValid = false;
this.listBox1.Items.Clear();
this.stopping = false;
await Task.Factory.StartNew(() =>
{
//拍照获取Xend和Yend的偏移
Machine.Instance.Robot.MoveSafeZAndReply();
Machine.Instance.Robot.MovePosXYAndReply(this.origin);
PointD delta0 = this.CaptrueDelta().Item1;
Machine.Instance.Robot.MovePosXYAndReply(this.hend.X, this.origin.Y);
PointD delta1 = this.CaptrueDelta().Item1;
//计算棋盘旋转矩阵
double sinth = (delta1.Y - delta0.Y) / ((int)(this.hend.DistanceTo(this.origin) / interval) * interval);
double baseX_x = interval *Math.Sqrt(1 - sinth *sinth);
double baseX_y = interval *sinth;
double baseY_x = -baseX_y;
double baseY_y = baseX_x;
//执行采样过程
for (int i = 0; i < this.rowNum; i++)
{
for (int j = 0; j < this.colNum; j++)
{
if (this.stopping)
{
break;
}
Tuple <PointD, double> delta;
//计算棋盘拍照位置
double predX = j * baseX_x + i * baseY_x;
double predY = j * baseX_y + i * baseY_y;
PointD p = this.origin + delta0 + new PointD(predX, predY);
Machine.Instance.Robot.MovePosXYAndReply(p);
//拍照计算偏差
delta = this.CaptrueDelta();
double realX = p.X - delta.Item1.X;
double realY = p.Y - delta.Item1.Y;
//添加结果数据
MapPoint mp = new MapPoint()
{
RealX = realX,
RealY = realY,
RobotX = p.X,
RobotY = p.Y,
IndexX = j,
IndexY = i,
R = delta.Item2,
Ok = true
};
if (mp.R > 1.1 || mp.R < 0.9)
{
mp.Ok = false;
}
Machine.Instance.Robot.CalibMapPrm.Items.Add(mp);
this.BeginInvoke(new MethodInvoker(() =>
{
this.listBox1.Items.Add(mp);
}));
//拍照后延时
Thread.Sleep(this.dwellTime);
}
}
Machine.Instance.Robot.MovePosXYAndReply(this.origin);
});
break;
case 2:
this.LblTitle.Text = "Doing calibration...";
await Task.Factory.StartNew(() =>
{
//采用双线性插值算法
CalibMap.creatNewClbMap(colNum, rowNum, interval);
foreach (var item in Machine.Instance.Robot.CalibMapPrm.Items)
{
CalibMap.addPointData(item.RealX, item.RealY, item.RobotX, item.RobotY, item.IndexX, item.IndexY);
}
//采用神经网络算法
Machine.Instance.Robot.InitRBF2D();
});
break;
}
this.flag++;
this.UpdateByFlag();
}
