diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3a0f5d3..b3f37b6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -72,7 +72,8 @@ import frc4388.robot.subsystems.LED; // import frc4388.robot.subsystems.ShooterAim_1; // import frc4388.robot.subsystems.ShooterHood_1; // import frc4388.robot.subsystems.Storage; -import frc4388.robot.subsystems.Vision; +// import frc4388.robot.subsystems.Vision; +import frc4388.robot.subsystems.VisionOdomotry; // import frc4388.robot.subsystems.Storage.StorageMode; import frc4388.utility.controller.ButtonFox; import frc4388.utility.controller.IHandController; @@ -101,7 +102,7 @@ public class RobotContainer { private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40); private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40); // public final LimeLight m_robotLime = new LimeLight(); - public final Vision m_robotVision = new Vision(); + public final VisionOdomotry m_robotVision = new VisionOdomotry(); /* Controllers */ public boolean isGS = false; @@ -231,7 +232,7 @@ public class RobotContainer { // .whenPressed(new SetShooterToOdo(m_robotShooterAim, m_robotDrive)); new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whenPressed(new VisionUpdateOdometry(m_robotVision));//, m_robotShooterAim, m_robotDrive)); + .whenPressed(() -> m_robotVision.getVisionOdometry()); // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); // // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood)); diff --git a/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java b/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java index fe6c646..18ae4cc 100644 --- a/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java +++ b/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java @@ -26,10 +26,11 @@ import frc4388.robot.Constants.VOPConstants; import frc4388.robot.Constants.VisionConstants; // import frc4388.robot.subsystems.Drive; // import frc4388.robot.subsystems.ShooterAim_1; -import frc4388.robot.subsystems.Vision; +import frc4388.robot.subsystems.VisionOdometry; +import frc4388.utility.VisionObscuredException; public class VisionUpdateOdometry extends CommandBase { - private Vision m_limeLight; + private VisionOdometry m_limeLight; // private ShooterAim_1 m_shooterAim; // private Drive m_driveTrain; @@ -45,7 +46,7 @@ public class VisionUpdateOdometry extends CommandBase { * @param shooterAim replace with Turret subsystem for integration with 2022 * @param driveTrain replace with Swerve subsystem for integration with 2022 */ - public VisionUpdateOdometry(Vision limeLight) { + public VisionUpdateOdometry(VisionOdometry limeLight) { m_limeLight = limeLight; // m_shooterAim = shooterAim; // m_driveTrain = driveTrain; @@ -66,7 +67,7 @@ public class VisionUpdateOdometry extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { + public void execute() throws VisionObscuredException { m_limeLight.setLEDs(true); // m_limeLight.changePipeline(5); @@ -75,10 +76,9 @@ public class VisionUpdateOdometry extends CommandBase { // Debug power off m_limeLight.setLEDs(false); - if(!(screenPoints != null && screenPoints.size() >= 3)) { - System.err.println("Vision Update Odometry Error: Not enough points"); + if(screenPoints.size() < 3) { m_limeLight.setLEDs(false); - return; + throw new VisionObscuredException("Not enough vision points available"); } ArrayList points3d = get3dPoints(screenPoints); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java deleted file mode 100644 index 2c67e37..0000000 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ /dev/null @@ -1,60 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; -import java.util.ArrayList; -import java.util.List; - -import org.opencv.core.Point; -import org.photonvision.PhotonCamera; -import org.photonvision.common.hardware.VisionLEDMode; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.VOPConstants; -import frc4388.robot.Constants.VisionConstants; - -public class Vision extends SubsystemBase { - private PhotonCamera m_camera; - // roborio ip & port: 10.43.88.2:1735 - - public Vision() { - m_camera = new PhotonCamera(VOPConstants.NAME); - } - - public ArrayList getTargetPoints() { - PhotonPipelineResult result = m_camera.getLatestResult(); - - if(!result.hasTargets()) - return null; - - ArrayList points = new ArrayList<>(); - - for(PhotonTrackedTarget target : result.getTargets()) { - List corners = target.getCorners(); - - double centerY = 0; - for(TargetCorner corner : corners) { - centerY += corner.y; - } - centerY /= corners.size(); - - for(TargetCorner corner : corners) { - if(corner.y <= centerY) - points.add(new Point(corner.x, VOPConstants.LIME_VIXELS - corner.y)); - } - } - - return points; - } - - public void setLEDs(boolean on) { - m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff); - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java new file mode 100644 index 0000000..7bcb884 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -0,0 +1,230 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.subsystems; + +import java.util.ArrayList; +import java.util.List; + +import org.opencv.core.Point; +import org.opencv.core.Point3; +import org.photonvision.PhotonCamera; +import org.photonvision.common.hardware.VisionLEDMode; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.VOPConstants; +import frc4388.robot.Constants.VisionConstants; +import frc4388.utility.VisionObscuredException; + +/** Represents the limelight and odometry related functionality + * @author Daniel Thomas McGrath + */ +public class VisionOdometry extends SubsystemBase { + // roborio ip & port: 10.43.88.2:1735 + private PhotonCamera m_camera; + + private VOPDrive m_drive; // replace with swerve drive subsystem + private VOPShooter m_shooter; // replace with turret subsystem + + /** Creates VisionOdometry + * + * @param drive + * @param shooter + */ + public VisionOdometry(VOPDrive drive, VOPShooter shooter) { + m_camera = new PhotonCamera(VOPConstants.NAME); + m_drive = drive; + m_shooter = shooter; + } + + /** Gets the vision points from the limelight + *

+ * Breaks down targets into 4 corners and uses the top 2 points + * + * @return Vision points on the rim of the target in screen space + */ + public ArrayList getTargetPoints() { + PhotonPipelineResult result = m_camera.getLatestResult(); + + if(!result.hasTargets()) + return new ArrayList(); + + ArrayList points = new ArrayList<>(); + + for(PhotonTrackedTarget target : result.getTargets()) { + List corners = target.getCorners(); + + double centerY = 0; + for(TargetCorner corner : corners) { + centerY += corner.y; + } + centerY /= corners.size(); + + for(TargetCorner corner : corners) { + if(corner.y <= centerY) + points.add(new Point(corner.x, VOPConstants.LIME_VIXELS - corner.y)); + } + } + + return points; + } + + /** Sets LEDs on or off (duh) + * + * @param on LED state + */ + public void setLEDs(boolean on) { + m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff); + } + + /** Gets estimated odometry based on limelight data + * + * @return The estimated odometry pose, including gyro rotation + * @throws VisionObscuredException + */ + public Pose2d getVisionOdometry() throws VisionObscuredException { + ArrayList screenPoints = getTargetPoints(); + + if(screenPoints.size() < 3) + throw new VisionObscuredException("Not enough vision points available"); + + ArrayList points3d = get3dPoints(screenPoints); + ArrayList points = topView(points3d); + + Point guess = averagePoint(points); + + for(int i = 0; i < 30; i++) { + guess = iterateGuess(guess, points); + } + + // TODO rotate guess for shooter & gyro + + SmartDashboard.putNumber("Vision ODO x: ", guess.x); + SmartDashboard.putNumber("Vision ODO y: ", guess.y); + + Pose2d odometryPose = new Pose2d(0, 0, new Rotation2d(0)); + + return odometryPose; + } + + /** Reverse 3d projects target points from screen coordinates to 3d space + *

+ * Uses the known height of the target to project points + * + * @param points2d Vision points on the rim of the target in screen space + * @return Reverse 3d projected points + */ + public static final ArrayList get3dPoints(ArrayList points2d) { + ArrayList points3d = new ArrayList<>(); + + for(Point point2d : points2d) { + double y_rot = point2d.y / VOPConstants.LIME_VIXELS; + y_rot *= Math.toRadians(VOPConstants.V_FOV); + y_rot -= Math.toRadians(VOPConstants.V_FOV) / 2; + y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); + + double x_rot = point2d.x / VOPConstants.LIME_HIXELS; + x_rot *= Math.toRadians(VOPConstants.H_FOV); + x_rot -= Math.toRadians(VOPConstants.H_FOV) / 2; + + double z = VOPConstants.TARGET_HEIGHT / Math.tan(y_rot); + double x = z * Math.tan(x_rot); + double y = VOPConstants.TARGET_HEIGHT; + + points3d.add(new Point3(x, y, z)); + } + + return points3d; + } + + /** Flattens 3d points from above + * + * @param points3d 3d points along the target rim + * @return An array of flattened 3d points + */ + public static final ArrayList topView(ArrayList points3d) { + ArrayList points = new ArrayList<>(); + + for(Point3 point3d : points3d) { + points.add(new Point(point3d.x, point3d.z)); + } + + return points; + } + + /** Finds the average point from an array of points + * + * @param points The points the average will be taken from + * @return The average point + */ + public static final Point averagePoint(ArrayList points) { + Point average = new Point(0, 0); + for(Point point : points) { + average.x += point.x; + average.y += point.y; + } + + average.x /= points.size(); + average.y /= points.size(); + + return average; + } + + /** Iterates the current guess for the vision center (relative to the limelight) + * based on points on the rim of the target + *

+ * The guess is iterated by finding the current average vector error between the guess + * and the circlePoints, assuming that the guess should be a constant radius from each point + * + * @param guess The current estimate for the vision center + * @param circlePoints Vision points along the rim of the target + * @return The guess after iteration + */ + public static final Point iterateGuess(Point guess, ArrayList circlePoints) { + Point totalDiff = new Point(0, 0); + + for(Point circlePoint : circlePoints) { + double angle = Math.atan((guess.y - circlePoint.y) / (guess.x - circlePoint.x)); + angle = correctQuadrent(angle, guess, circlePoint); + + Point estimate = new Point(); + estimate.x = VOPConstants.TARGET_RADIUS * Math.cos(angle) + guess.x; + estimate.y = VOPConstants.TARGET_RADIUS * Math.sin(angle) + guess.y; + + Point diff = new Point(estimate.x - circlePoint.x, estimate.y - circlePoint.y); + totalDiff.x += diff.x; + totalDiff.y += diff.y; + } + + totalDiff.x /= circlePoints.size(); + totalDiff.y /= circlePoints.size(); + + return new Point(guess.x - totalDiff.x, guess.y - totalDiff.y); + } + + /** Corrects the angle from the current center estimate to a point on the target rim + * for multiple quadrents + * + * @param angle The angle to be corrected + * @param guess The current guess for the vision center + * @param circlePoint A point along the target rim + * @return The angle corrected for the quadrent + */ + public static final double correctQuadrent(double angle, Point guess, Point circlePoint) { + if(circlePoint.x - guess.x < 0) { + return angle - Math.PI; + } + + return angle; + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/VisionObscuredException.java b/src/main/java/frc4388/utility/VisionObscuredException.java index e69de29..6503b9a 100644 --- a/src/main/java/frc4388/utility/VisionObscuredException.java +++ b/src/main/java/frc4388/utility/VisionObscuredException.java @@ -0,0 +1,22 @@ +package frc4388.utility; + +/** Exception that occurs if the limelight can't see enough points + * @author Daniel Thomas McGrath + */ +public class VisionObscuredException extends RuntimeException { + public VisionObscuredException() { + super(); + } + + public VisionObscuredException(String message) { + super(message); + } + + public VisionObscuredException(String message, Throwable cause) { + super(message, cause); + } + + public VisionObscuredException(Throwable cause) { + super(cause); + } +} \ No newline at end of file