static decimal Distance(Fiducial f1, Fiducial f2)
{
decimal x = f1.realX - f2.realX;
decimal y = f1.realY - f2.realY;
return(x * x + y * y);
}
static decimal Distance(Placement p, Fiducial f)
{
decimal x = f.posX - p.posX;
decimal y = f.posY - p.posY;
return(x * x + y * y);
}
static void updateBoard()
{
if (board == null || board.steps == null || board.steps.Count == 0 || board.fiducials == null || board.fiducials.Count == 0)
{
return;
}
if (state.lastStep <= 0)
{
state.lastStep = 0;
}
if (state.lastStep >= board.steps.Count - 1)
{
state.lastStep = board.steps.Count - 1;
}
if (state.lastFid <= 0)
{
state.lastFid = 0;
}
if (state.lastFid >= board.fiducials.Count - 1)
{
state.lastFid = board.fiducials.Count - 1;
}
Placement pl = board.steps[state.lastStep];
Native.DrawText(820, 275, 1, 0, pl.reference + " " + pl.posX + " " + pl.posY + " " + pl.angle + " ");
Fiducial f = board.fiducials[state.lastFid];
Native.DrawText(820, 300, 1, 0, f.id + " " + f.posX + " " + f.posY + " " + f.realX + " " + f.realY + " ");
}
static void MoveToFid(int fi)
{
Fiducial f = board.fiducials[fi];
var tf = boardPosToPnpPos(f.posX, f.posY);
channel.MoveTo(x: tf.Item1, y: tf.Item2, s: settings.speed);
updateState();
}
static Tuple <decimal, decimal> CalcPartRelative()
{
Placement pl = board.steps[state.lastStep];
var fds = (from f in board.fiducials select f).OrderBy((xf) => Distance(pl, xf)).ToList();
if (fds.Count > 3)
{
fds.RemoveRange(3, fds.Count - 3);
}
if (fds.Count == 2)
{
if (fds[0].posX > fds[1].posX)
{
Fiducial tmp = fds[0];
fds[0] = fds[1];
fds[1] = tmp;
}
Fiducial fe = new Fiducial();
fe.posX = fds[0].posX - (fds[1].posY - fds[0].posY);
fe.posY = fds[0].posY + (fds[1].posX - fds[0].posX);
fe.realX = fds[0].realX + fds[1].realY - fds[0].realY;
fe.realY = fds[0].realY + fds[1].realX - fds[0].realX;
fds.Add(fe);
}
var d1 = Distance(fds[0], fds[1]);
var d2 = Distance(fds[0], fds[2]);
var d3 = Distance(fds[2], fds[1]);
Fiducial fc, fx, fy;
if (d1 > d2)
{
if (d1 > d3)
{
fx = fds[0];
fy = fds[1];
fc = fds[2];
}
else
{
fx = fds[2];
fy = fds[1];
fc = fds[0];
}
}
else
{
if (d2 > d3)
{
fx = fds[0];
fy = fds[2];
fc = fds[1];
}
else
{
fx = fds[2];
fy = fds[1];
fc = fds[0];
}
}
if (Math.Abs(fy.posX - fc.posX) > Math.Abs(fx.posX - fc.posX))
{
Fiducial ftmp = fx;
fx = fy;
fy = ftmp;
}
decimal vx = fx.posX - fc.posX;
decimal vy = fy.posY - fc.posY;
decimal px = board.steps[state.lastStep].posX - fc.posX;
decimal py = board.steps[state.lastStep].posY - fc.posY;
decimal cx = px / vx;
decimal cy = py / vy;
decimal x = fc.realX + cx * (fx.realX - fc.realX) + cy * (fy.realX - fc.realX);
decimal y = fc.realY + cx * (fx.realY - fc.realY) + cy * (fy.realY - fc.realY);
Native.DrawText(820, 530, 1, 0, Decimal.Round(x - state.lastX, 2) + " " + Decimal.Round(y - state.lastY, 2));
return(new Tuple <decimal, decimal>(x, y));
}
static void BoardCallback(long data)
{
if (board == null || board.steps == null || board.steps.Count == 0 || board.fiducials == null || board.fiducials.Count == 0)
{
return;
}
switch (data & 255)
{
case 1:
state.lastStep--;
if (state.lastStep <= 0)
{
state.lastStep = 0;
}
break;
case 2:
state.lastStep++;
if (state.lastStep >= board.steps.Count - 1)
{
state.lastStep = board.steps.Count - 1;
}
break;
case 3:
Placement pl = board.steps[state.lastStep];
var tp = boardPosToPnpPos(pl.posX, pl.posY);
channel.MoveTo(x: tp.Item1, y: tp.Item2, s: settings.speed);
updateState();
break;
case 4:
state.lastFid--;
if (state.lastFid <= 0)
{
state.lastFid = 0;
}
break;
case 5:
state.lastFid++;
if (state.lastFid >= board.fiducials.Count - 1)
{
state.lastFid = board.fiducials.Count - 1;
}
break;
case 6:
Run(() => MoveToFid(state.lastFid));
break;
case 7:
Run(() => AlignFid(state.lastFid));
break;
case 8:
var xy = CalcPartRelative();
channel.MoveTo(x: xy.Item1, y: xy.Item2, s: settings.speed);
updateState();
break;
case 9:
Fiducial f = board.fiducials[state.lastFid];
var tf = boardPosToPnpPos(f.posX, f.posY);
settings.boardZeroX += state.lastX - tf.Item1;
settings.boardZeroY += state.lastY - tf.Item2;
break;
case 10:
Run(() => MoveToFidReal(state.lastFid));
break;
case 11:
Native.Vec2 retval = new Native.Vec2();
Native.findsymmetry(0, 50, 50, 100, 100, retval);
Native.findsymmetry(1, 50, 50, 100, 100, retval);
Native.save_screen("test.bmp");
break;
case 20:
Run(AlignUpCam);
break;
case 21:
Run(CorrectUpCam);
break;
}
updateBoard();
}