// TODO: Throw an Exception if vision fails. private Location getVisionOffsets(Head head, Location pickLocation) throws Exception { logger.debug("getVisionOffsets({}, {})", head.getName(), pickLocation); // Find the Camera to be used for vision // TODO: Consider caching this Camera camera = null; for (Camera c : head.getCameras()) { if (c.getVisionProvider() != null) { camera = c; } } if (camera == null) { throw new Exception("No vision capable camera found on head."); } head.moveToSafeZ(1.0); // Position the camera over the pick location. logger.debug("Move camera to pick location."); camera.moveTo(pickLocation, 1.0); // Move the camera to be in focus over the pick location. // head.moveTo(head.getX(), head.getY(), z, head.getC()); // Settle the camera Thread.sleep(camera.getSettleTimeMs()); VisionProvider visionProvider = camera.getVisionProvider(); Rectangle aoi = getVision().getAreaOfInterest(); // Perform the template match logger.debug("Perform template match."); Point[] matchingPoints = visionProvider.locateTemplateMatches( aoi.getX(), aoi.getY(), aoi.getWidth(), aoi.getHeight(), 0, 0, vision.getTemplateImage()); // Get the best match from the array Point match = matchingPoints[0]; // match now contains the position, in pixels, from the top left corner // of the image to the top left corner of the match. We are interested in // knowing how far from the center of the image the center of the match is. BufferedImage image = camera.capture(); double imageWidth = image.getWidth(); double imageHeight = image.getHeight(); double templateWidth = vision.getTemplateImage().getWidth(); double templateHeight = vision.getTemplateImage().getHeight(); double matchX = match.x; double matchY = match.y; logger.debug("matchX {}, matchY {}", matchX, matchY); // Adjust the match x and y to be at the center of the match instead of // the top left corner. matchX += (templateWidth / 2); matchY += (templateHeight / 2); logger.debug("centered matchX {}, matchY {}", matchX, matchY); // Calculate the difference between the center of the image to the // center of the match. double offsetX = (imageWidth / 2) - matchX; double offsetY = (imageHeight / 2) - matchY; logger.debug("offsetX {}, offsetY {}", offsetX, offsetY); // Invert the Y offset because images count top to bottom and the Y // axis of the machine counts bottom to top. offsetY *= -1; logger.debug("negated offsetX {}, offsetY {}", offsetX, offsetY); // And convert pixels to units Location unitsPerPixel = camera.getUnitsPerPixel(); offsetX *= unitsPerPixel.getX(); offsetY *= unitsPerPixel.getY(); logger.debug("final, in camera units offsetX {}, offsetY {}", offsetX, offsetY); return new Location(unitsPerPixel.getUnits(), offsetX, offsetY, 0, 0); }