/*
Copyright (C) 2011 Jason von Nieda <jason@vonnieda.org>
This file is part of OpenPnP.
OpenPnP is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
OpenPnP is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with OpenPnP. If not, see <http://www.gnu.org/licenses/>.
For more information about OpenPnP visit http://openpnp.org
*
* Change Log:
* 03/10/2012 Ami: Add four points best fit algorithm.
* - Takes the two angles of the two opposing corners (the diagonals) from the placements and compare it to the indicated values.
* - These are the starting point for the binary search, to find the lowest error.
* - Each iteration the mid-point angle is also taken, and all three are evaluated.
* - Offset is re-calculated after rotation and averaged
*/
package org.openpnp.gui.processes;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import org.openpnp.gui.JobPanel;
import org.openpnp.gui.MainFrame;
import org.openpnp.gui.support.MessageBoxes;
import org.openpnp.model.Board.Side;
import org.openpnp.model.Configuration;
import org.openpnp.model.Location;
import org.openpnp.model.Placement;
import org.openpnp.model.Point;
import org.openpnp.spi.Camera;
import org.openpnp.spi.Head;
import org.openpnp.util.MovableUtils;
import org.openpnp.util.Utils2D;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
/**
* Guides the user through the two point board location operation using
* step by step instructions.
*
* TODO: Select the right camera on startup and then disable the CameraPanel while active.
* TODO: Disable the BoardLocation table while active.
*/
public class FourPlacementBoardLocationProcess {
private static final Logger logger = LoggerFactory.getLogger(FourPlacementBoardLocationProcess.class);
private final MainFrame mainFrame;
private final JobPanel jobPanel;
private int step = -1;
private String[] instructions = new String[] {
"<html><body>Pick an easily identifiable placement near the TOP-LEFT corner of the board. Select it in the table below and move the camera's crosshairs to it's center location. Click Next to continue.</body></html>",
"<html><body>Next, pick another placement on the BOTTOM-RIGHT corner of the board, select it in the table below and move the camera's crosshairs to it's center location. Click Next to continue.</body></html>",
"<html><body>And now, pick another placement on the TOP-RIGHT corner of the board, select it in the table below and move the camera's crosshairs to it's center location. Click Next to continue.</body></html>",
"<html><body>Last, pick another placement on the BOTTOM-LEFT corner of the board, select it in the table below and move the camera's crosshairs to it's center location. Click Next to continue.</body></html>",
"<html><body>The board's location and rotation has been set. Click Finish to position the camera at the board's origin, or Cancel to quit.</body></html>",
};
private Placement placementA, placementB, placementC, placementD;
private Location placementLocationA, placementLocationB, placementLocationC, placementLocationD;
public FourPlacementBoardLocationProcess(MainFrame mainFrame, JobPanel jobPanel) {
this.mainFrame = mainFrame;
this.jobPanel = jobPanel;
advance();
}
private void advance() {
boolean stepResult = true;
if (step == 0) {
stepResult = step1();
}
else if (step == 1) {
stepResult = step2();
}
else if (step == 2) {
stepResult = step3();
}
else if (step == 3) {
stepResult = step4();
}
else if (step == 4) {
stepResult = step5();
}
if (!stepResult) {
return;
}
step++;
if (step == 5) {
mainFrame.hideInstructions();
}
else {
String title = String.format("Set Board Location (%d / 5)", step + 1);
mainFrame.showInstructions(
title,
instructions[step],
true,
true,
step == 4 ? "Finish" : "Next",
cancelActionListener,
proceedActionListener);
}
}
private boolean step1() {
placementLocationA = MainFrame.cameraPanel.getSelectedCameraLocation();
if (placementLocationA == null) {
MessageBoxes.errorBox(mainFrame, "Error", "Please position the camera.");
return false;
}
placementA = jobPanel.getSelectedPlacement();
if (placementA == null) {
MessageBoxes.errorBox(mainFrame, "Error", "Please select a placement.");
return false;
}
return true;
}
private boolean step2() {
placementLocationB = MainFrame.cameraPanel.getSelectedCameraLocation();
if (placementLocationB == null) {
MessageBoxes.errorBox(mainFrame, "Error", "Please position the camera.");
return false;
}
placementB = jobPanel.getSelectedPlacement();
if (placementB == null) {
MessageBoxes.errorBox(mainFrame, "Error", "Please select a placement.");
return false;
}
return true;
}
private boolean step3() {
placementLocationC = MainFrame.cameraPanel.getSelectedCameraLocation();
if (placementLocationC == null) {
MessageBoxes.errorBox(mainFrame, "Error", "Please position the camera.");
return false;
}
placementC = jobPanel.getSelectedPlacement();
if (placementC == null) {
MessageBoxes.errorBox(mainFrame, "Error", "Please select a placement.");
return false;
}
return true;
}
private boolean step4() {
placementLocationD = MainFrame.cameraPanel.getSelectedCameraLocation();
if (placementLocationD == null) {
MessageBoxes.errorBox(mainFrame, "Error", "Please position the camera.");
return false;
}
placementD = jobPanel.getSelectedPlacement();
if (placementD == null || placementD == placementC) {
MessageBoxes.errorBox(mainFrame, "Error", "Please select a second placement.");
return false;
}
if ((placementA.getSide() != placementB.getSide()) || (placementC.getSide() != placementD.getSide())){
MessageBoxes.errorBox(mainFrame, "Error", "Both placements must be on the same side of the board.");
return false;
}
// Get the Locations we'll be using and convert to system units.
Location boardLocationA = placementLocationA.convertToUnits(Configuration.get().getSystemUnits());
Location placementLocationA = placementA.getLocation().convertToUnits(Configuration.get().getSystemUnits());
Location boardLocationB = placementLocationB.convertToUnits(Configuration.get().getSystemUnits());
Location placementLocationB = placementB.getLocation().convertToUnits(Configuration.get().getSystemUnits());
Location boardLocationC = placementLocationC.convertToUnits(Configuration.get().getSystemUnits());
Location placementLocationC = placementC.getLocation().convertToUnits(Configuration.get().getSystemUnits());
Location boardLocationD = placementLocationD.convertToUnits(Configuration.get().getSystemUnits());
Location placementLocationD = placementD.getLocation().convertToUnits(Configuration.get().getSystemUnits());
// If the placements are on the Bottom of the board we need to invert X
if (placementA.getSide() == Side.Bottom) {
// boardLocationA = boardLocationA.invert(true, false, false, false);
placementLocationA = placementLocationA.invert(true, false, false, false);
// boardLocationB = boardLocationB.invert(true, false, false, false);
placementLocationB = placementLocationB.invert(true, false, false, false);
}
if (placementC.getSide() == Side.Bottom) {
// boardLocationA = boardLocationA.invert(true, false, false, false);
placementLocationC = placementLocationC.invert(true, false, false, false);
// boardLocationB = boardLocationB.invert(true, false, false, false);
placementLocationD = placementLocationD.invert(true, false, false, false);
}
logger.debug(String.format("locate"));
logger.debug(String.format("%s - %s", boardLocationA,
placementLocationA));
logger.debug(String.format("%s - %s", boardLocationB,
placementLocationB));
logger.debug(String.format("%s - %s", boardLocationC,
placementLocationC));
logger.debug(String.format("%s - %s", boardLocationD,
placementLocationD));
double x1 = placementLocationA.getX();
double y1 = placementLocationA.getY();
double x2 = placementLocationB.getX();
double y2 = placementLocationB.getY();
// Center of the placement points used for rotation
double centerX = (x1+x2)/2;
double centerY = (y1+y2)/2;
// Calculate the expected angle between the two coordinates, based
// on their locations in the placement.
double expectedAngle = Math.atan2(y1 - y2, x1 - x2);
expectedAngle = Math.toDegrees(expectedAngle);
logger.debug("expectedAngle A-B " + expectedAngle);
// Then calculate the actual angle between the two coordinates,
// based on the captured values.
x1 = boardLocationA.getX();
y1 = boardLocationA.getY();
x2 = boardLocationB.getX();
y2 = boardLocationB.getY();
double indicatedAngle = Math.atan2(y1 - y2, x1 - x2);
indicatedAngle = Math.toDegrees(indicatedAngle);
logger.debug("indicatedAngle A-B " + indicatedAngle);
// Subtract the difference and we have the angle that the board
// is rotated by.
double angleAB = indicatedAngle - expectedAngle ; // this is the rotation angle to be done
logger.debug("angle A-B " + angleAB);
// Now do the same for C-D
x1 = placementLocationC.getX();
y1 = placementLocationC.getY();
x2 = placementLocationD.getX();
y2 = placementLocationD.getY();
centerX += (x1+x2)/2;
centerY += (y1+y2)/2;
// Calculate the expected angle between the two coordinates, based
// on their locations in the placement.
expectedAngle = Math.atan2(y1 - y2, x1 - x2);
expectedAngle = Math.toDegrees(expectedAngle);
logger.debug("expectedAngle C-D " + expectedAngle);
// Then calculate the actual angle between the two coordinates,
// based on the captured values.
x1 = boardLocationC.getX();
y1 = boardLocationC.getY();
x2 = boardLocationD.getX();
y2 = boardLocationD.getY();
indicatedAngle = Math.atan2(y1 - y2, x1 - x2);
indicatedAngle = Math.toDegrees(indicatedAngle);
logger.debug("indicatedAngle C-D " + indicatedAngle);
// Subtract the difference and we have the angle that the board
// is rotated by.
double angleCD = indicatedAngle - expectedAngle ; // this is the rotation angle to be done
logger.debug("angle C-D " + angleCD);
Point center = new Point(centerX/2,centerY/2); // This is the center point of the four board used for rotation
double dxAB = 0, dxCD = 0, dxMP = 0;
double dyAB = 0, dyCD = 0, dyMP = 0;
// Now we do binary search n-times between AngleAB and AngleCD to find lowest error
// This is up to as good as we want, I prefer for-loop than while-loop.
for(int i = 0; i< 50;++i)
{
// use angleAB to calculate the displacement necessary to get placementLocation to boardLocation.
// Each point will have slightly different value.
// Then we can tell the error resulted from using this angleAB.
Point A = new Point(placementLocationA.getX(),placementLocationA.getY());
A = Utils2D.rotateTranslateCenterPoint(A, angleAB,0,0,center);
Point B = new Point(placementLocationB.getX(),placementLocationB.getY());
B = Utils2D.rotateTranslateCenterPoint(B, angleAB,0,0,center);
Point C = new Point(placementLocationC.getX(),placementLocationC.getY());
C = Utils2D.rotateTranslateCenterPoint(C, angleAB,0,0,center);
Point D = new Point(placementLocationD.getX(),placementLocationD.getY());
D = Utils2D.rotateTranslateCenterPoint(D, angleAB,0,0,center);
double dA = (boardLocationA.getX() - A.getX());
double dB = (boardLocationB.getX() - B.getX());
double dC = (boardLocationC.getX() - C.getX());
double dD = (boardLocationD.getX() - D.getX());
// Take the average of the four
dxAB = (dA + dB + dC + dD)/4;
double errorAB = Math.abs(dxAB- dA) + Math.abs(dxAB- dB) + Math.abs(dxAB- dC) + Math.abs(dxAB- dD);
dA = (boardLocationA.getY() - A.getY());
dB = (boardLocationB.getY() - B.getY());
dC = (boardLocationC.getY() - C.getY());
dD = (boardLocationD.getY() - D.getY());
// Take the average of the four
dyAB = (dA + dB + dC + dD)/4;
errorAB += Math.abs(dyAB- dA) + Math.abs(dyAB- dB) + Math.abs(dyAB- dC) + Math.abs(dyAB- dD); // Accumulate the error
// Now do the same using angleCD, find the error caused by angleCD
A = new Point(placementLocationA.getX(),placementLocationA.getY());
A = Utils2D.rotateTranslateCenterPoint(A, angleCD,0,0,center);
B = new Point(placementLocationB.getX(),placementLocationB.getY());
B = Utils2D.rotateTranslateCenterPoint(B, angleCD,0,0,center);
C = new Point(placementLocationC.getX(),placementLocationC.getY());
C = Utils2D.rotateTranslateCenterPoint(C, angleCD,0,0,center);
D = new Point(placementLocationD.getX(),placementLocationD.getY());
D = Utils2D.rotateTranslateCenterPoint(D, angleCD,0,0,center);
dA = (boardLocationA.getX() - A.getX());
dB = (boardLocationB.getX() - B.getX());
dC = (boardLocationC.getX() - C.getX());
dD = (boardLocationD.getX() - D.getX());
// Take the average of the four
dxCD = (dA + dB + dC + dD)/4;
double errorCD = Math.abs(dxCD- dA) + Math.abs(dxCD- dB) + Math.abs(dxCD- dC) + Math.abs(dxCD- dD);
dA = (boardLocationA.getY() - A.getY());
dB = (boardLocationB.getY() - B.getY());
dC = (boardLocationC.getY() - C.getY());
dD = (boardLocationD.getY() - D.getY());
// Take the average of the four
dyCD = (dA + dB + dC + dD)/4;
errorCD += Math.abs(dyCD- dA) + Math.abs(dyCD- dB) + Math.abs(dyCD- dC) + Math.abs(dyCD- dD); // Accumulate the error
// Now take the mid-point between the two angles,
// and do the same math
double angleMP = (angleAB + angleCD)/2; // MP = mid-point angle between angleAB and angleCD
double deltaAngle = Math.abs(angleAB - angleCD);
A = new Point(placementLocationA.getX(),placementLocationA.getY());
A = Utils2D.rotateTranslateCenterPoint(A, angleMP,0,0,center);
B = new Point(placementLocationB.getX(),placementLocationB.getY());
B = Utils2D.rotateTranslateCenterPoint(B, angleMP,0,0,center);
C = new Point(placementLocationC.getX(),placementLocationC.getY());
C = Utils2D.rotateTranslateCenterPoint(C, angleMP,0,0,center);
D = new Point(placementLocationD.getX(),placementLocationD.getY());
D = Utils2D.rotateTranslateCenterPoint(D, angleMP,0,0,center);
dA = (boardLocationA.getX() - A.getX());
dB = (boardLocationB.getX() - B.getX());
dC = (boardLocationC.getX() - C.getX());
dD = (boardLocationD.getX() - D.getX());
// Take the average of the four
dxMP = (dA + dB + dC + dD)/4;
double errorMP = Math.abs(dxMP- dA) + Math.abs(dxMP- dB) + Math.abs(dxMP- dC) + Math.abs(dxMP- dD);
dA = (boardLocationA.getY() - A.getY());
dB = (boardLocationB.getY() - B.getY());
dC = (boardLocationC.getY() - C.getY());
dD = (boardLocationD.getY() - D.getY());
// Take the average of the four
dyMP = (dA + dB + dC + dD)/4;
errorMP += Math.abs(dyMP- dA) + Math.abs(dyMP- dB) + Math.abs(dyMP- dC) + Math.abs(dyMP- dD); // Accumulate the error
// This is gradient descend searching for local minima (hopefully) between angleAB and angle CD
logger.debug(String.format("Error AB=%g vs MP=%g vs CD=%g ", errorAB, errorMP, errorCD));
if(errorAB < errorCD)
{
if(errorMP > errorAB) // ok, so no local minima between AB and CD, let's search beyond AB
{
angleMP = angleAB; // use as temporary, this is MP of the previous cycle
// Beyond means both ways
if(angleAB > angleCD)
angleAB += deltaAngle;
else
angleAB -= deltaAngle;
angleCD = angleMP;
}
else
{
// Local minima is for sure between AB and CD,
// best bet it's between MP and AB
// otherwise next step it will look on the other side (angleCD + deltaAngle)
angleCD = angleMP;
}
}
else
{
if(errorMP > errorCD) // ok so no local minima between AB and CD, let's search beyond CD
{
angleMP = angleCD; // use as temporary, this is MP of the previous cycle
// Beyond means both ways
if(angleCD > angleAB)
angleCD += deltaAngle;
else
angleCD -= deltaAngle;
angleAB = angleCD;
}
else
{
// local minima is between AB and CD,
// best bet it's between MP and CD, so set AB to the mid point (MP)
// otherwise next step it will look on the other side (angleCD + deltaAngle)
angleAB = angleMP;
}
}
}
double angle = (angleAB + angleCD)/2; // take the average
logger.debug("angle final " + angle);
// Take the average of the four
//double dx = (dxAB + dxCD)/2;
//double dy = (dyAB + dyCD)/2;
double dx = dxMP;
double dy = dyMP;
logger.debug(String.format("dx %f, dy %f", dx, dy));
Location boardLocation = new Location(Configuration.get()
.getSystemUnits(), dx, dy, 0, angle );
Location oldBoardLocation = jobPanel.getSelectedBoardLocation().getLocation();
oldBoardLocation = oldBoardLocation.convertToUnits(boardLocation.getUnits());
boardLocation = boardLocation.derive(null, null, oldBoardLocation.getZ(), null);
jobPanel.getSelectedBoardLocation().setLocation(boardLocation);
// TODO: Set Board center point when center points are finished.
// jobPanel.getSelectedBoardLocation().setCenter(center);
jobPanel.refreshSelectedBoardRow();
return true;
}
private boolean step5() {
MainFrame.machineControlsPanel.submitMachineTask(new Runnable() {
public void run() {
Head head = Configuration.get().getMachine().getHeads().get(0);
try {
Camera camera = MainFrame.cameraPanel
.getSelectedCamera();
Location location = jobPanel.getSelectedBoardLocation()
.getLocation();
MovableUtils.moveToLocationAtSafeZ(camera, location, 1.0);
}
catch (Exception e) {
MessageBoxes.errorBox(mainFrame,
"Move Error", e);
}
}
});
return true;
}
private void cancel() {
mainFrame.hideInstructions();
}
private final ActionListener proceedActionListener = new ActionListener() {
@Override
public void actionPerformed(ActionEvent e) {
advance();
}
};
private final ActionListener cancelActionListener = new ActionListener() {
@Override
public void actionPerformed(ActionEvent e) {
cancel();
}
};
}