// +x is right // +y is down public double distanceOffset(double x, double y) { // Image height (px) double ih = Camera.RESOLUTION_Y_PIXELS; // Image width (px) double iw = Camera.RESOLUTION_X_PIXELS; // Signed distance from center, in y direction (px) double hv = y - ih / 2; // signed distance from center, in x direction (px) double xv = iw / 2 - x; // Field of view (rad) double fov = Camera.FOV_Y_RADIANS; // Distance to virtual image plane (px) double dv = (1.0 * ih) / (2 * Math.tan(fov / 2)); // Height from camera to goal (in) double hr = 77.75; // Angle of inclination of the camera (rad) double ac = 41 * Math.PI / 180; // Distance offset from camera to center of rotation (in) double od = 12; // Horizontal offset from camera to center of rotation (in) double ox = 9.25; // Real distance from center of rotation to goal (in) double dr = hr / Math.tan(ac - Math.atan(hv / dv)) + od; // Real offset from center of rotation to goal (in) double xr = dr * xv / dv + ox; // Angle from center of rotation to goal (rad) double a = Math.atan(xr / dr); // Distance from center of rotation to goal (in) double d = Math.hypot(dr, xr); System.out.println("Real Angle: " + a * 180 / Math.PI); System.out.println("Real dist: " + d); return a; }
private void targetAnalysis( PolygonCv foundTarget, boolean printToNetWorkTable) { // tells the robo info about the target double offsetXDegrees, offsetXDegreesIdeal, targetDistanceInches, baseDistanceInches, cameraAngleElevationRadians, targetAngleRadians, targetAngleFactor; // calculates how far off center the target is from the center of the camera // offsetXDegrees = Math.atan((400 - foundTarget.getCenterX()) * // Math.tan(Camera.FOV_X_RADIANS/2) / (Camera.RESOLUTION_X_PIXELS/2)); offsetXDegrees = Math.atan( (400 - foundTarget.getMinX()) * Math.tan(Camera.FOV_X_RADIANS / 2) / (Camera.RESOLUTION_X_PIXELS / 2)); // converts from radians (output of Math) to degrees offsetXDegrees = Math.toDegrees(offsetXDegrees); // used to determine size of the target in radians targetAngleFactor = 2 * Math.tan(Camera.FOV_X_RADIANS / 2) / Camera.RESOLUTION_X_PIXELS; /* //gets size of target in Radians targetAngleRadians = Math.atan(((Camera.RESOLUTION_X_PIXELS/2) - foundTarget.getMaxX()) * targetAngleFactor) - Math.atan(((Camera.RESOLUTION_X_PIXELS/2) - foundTarget.getMinX()) * targetAngleFactor); //gets degree value of top and bottom points, and finds difference //targetAngleRadians could be negative, compensates for this targetAngleRadians = Math.abs(targetAngleRadians); //gets distance to target targetDistanceInches = (Target.TAPE_WIDTH_INCHES / 2) / Math.tan(targetAngleRadians); */ // use perspective height rather than targetTapeHeight? // ye olde algorithm /* targetDistanceInches = (Target.TAPE_WIDTH_INCHES / 2) / Math.tan((Target.TAPE_WIDTH_INCHES / Camera.RESOLUTION_X_PIXELS) * (Camera.FOV_X_RADIANS / 2)); */ // new old algo double dpx = (Camera.RESOLUTION_X_PIXELS / 2) / Math.tan(Camera.FOV_X_RADIANS / 2); double tta = (foundTarget.getWidth() / 2) / dpx; targetDistanceInches = (Target.TAPE_WIDTH_INCHES) / Math.tan(tta); // System.out.println(dpx + " " + tta + " " + targetDistanceInches); // gets elevation of target to camera relative to ground cameraAngleElevationRadians = Math.asin((Target.TOWER_HEIGHT_INCHES - Camera.OFFSET_Y_INCHES) / targetDistanceInches); // gets distance to the base of the target baseDistanceInches = Math.cos(cameraAngleElevationRadians) * targetDistanceInches; /* gets 'ideal' target off center angle - because camera is to the side, * a perfectly zeroed robot will be slightly off from the camera */ // if(Camera.OFFSET_X_INCHES != 0) { // offsetXDegreesIdeal = Math.toDegrees(Math.atan(targetDistanceInches / // Camera.OFFSET_X_INCHES)); // } else { //will be positive if cameraCenterOffset is negative // offsetXDegreesIdeal = 0; // } // compensates for Ideal angle offset // offsetXDegrees = offsetXDegrees - offsetXDegreesIdeal; double camAngle = fovCalc.pixelFromCenterToDeg(foundTarget.getCenterX() - (Camera.RESOLUTION_X_PIXELS / 2)); double rot = -(robotAngleOffset - camAngle); // If rotation estimator enabled, use it as a better guess as how much to rotate if (rotEst != null) { // Guess at camera position somewhere between 75 and 120 inches from plane // containing target double distEst = (75 + 120) / 2; rot = rotEst.computeRotation(camAngle, distEst); } offsetXDegrees = rot; // TODO temporary substitution // determines if angle values are reasonable if (offsetXDegrees < (Camera.FOV_X_DEGREES / 2) && offsetXDegrees > (-Camera.FOV_X_DEGREES / 2) && printToNetWorkTable) { networkTable.putNumber( "OffCenterDegreesX", -Math.toDegrees(distanceOffset(foundTarget.getCenterX(), foundTarget.getCenterY()))); } System.out.println("Angle: " + offsetXDegrees); // writes calculated data to network tables System.out.println("Distance: " + baseDistanceInches); if (printToNetWorkTable) { networkTable.putNumber("DistanceToBase", baseDistanceInches); networkTable.putNumber("DistanceToTarget", targetDistanceInches); } // Newest Algorithm distanceOffset(foundTarget.getCenterX(), foundTarget.getCenterY()); }