diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 85a1bb1..cd97fe5 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -288,6 +288,8 @@ public final class Constants { public static final double LIME_VIXELS = 960; public static final double LIME_HIXELS = 720; + public static final double LIMELIGHT_RADIUS = 1.d; + public static final double SHOOTER_CORRECTION = 1.d; } public static final class OIConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b3f37b6..73cfd96 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -73,7 +73,7 @@ import frc4388.robot.subsystems.LED; // import frc4388.robot.subsystems.ShooterHood_1; // import frc4388.robot.subsystems.Storage; // import frc4388.robot.subsystems.Vision; -import frc4388.robot.subsystems.VisionOdomotry; +import frc4388.robot.subsystems.VisionOdometry; // import frc4388.robot.subsystems.Storage.StorageMode; import frc4388.utility.controller.ButtonFox; import frc4388.utility.controller.IHandController; @@ -102,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 VisionOdomotry m_robotVision = new VisionOdomotry(); + public final VisionOdometry m_robotVision = new VisionOdometry(); /* Controllers */ public boolean isGS = false; diff --git a/src/main/java/frc4388/robot/subsystems/VOPShooter.java b/src/main/java/frc4388/robot/subsystems/VOPShooter.java index 2dbb556..a42afb1 100644 --- a/src/main/java/frc4388/robot/subsystems/VOPShooter.java +++ b/src/main/java/frc4388/robot/subsystems/VOPShooter.java @@ -6,4 +6,8 @@ public class VOPShooter extends SubsystemBase { public VOPShooter() { } + + public double getShooterRotation() { + return 0; + } } \ 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 index 7bcb884..9b37066 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -107,12 +107,14 @@ public class VisionOdometry extends SubsystemBase { guess = iterateGuess(guess, points); } - // TODO rotate guess for shooter & gyro + guess = correctGuessForCenter(guess, m_shooter.getShooterRotation()); + guess = correctGuessForGyro(guess, m_drive.getRotation()); SmartDashboard.putNumber("Vision ODO x: ", guess.x); SmartDashboard.putNumber("Vision ODO y: ", guess.y); - Pose2d odometryPose = new Pose2d(0, 0, new Rotation2d(0)); + Rotation2d rotation = new Rotation2d(Math.toDegrees(m_drive.getRotation())); + Pose2d odometryPose = new Pose2d(guess.x, guess.y, rotation); return odometryPose; } @@ -212,6 +214,47 @@ public class VisionOdometry extends SubsystemBase { return new Point(guess.x - totalDiff.x, guess.y - totalDiff.y); } + /** Corrects odometry guess for shooter angle + * + * @param guess The current guess for the vision center + * @param gyroRotation The rotation to correct for + * @return The corrected odometry point + */ + public static final Point correctGuessForCenter(Point guess, double shooterRotation) { + Point corrected = new Point(guess.x, guess.y); + corrected.y += VOPConstants.LIMELIGHT_RADIUS; + + double dist = Math.hypot(guess.x, guess.y); + double angle = Math.tan(corrected.y / corrected.x); + angle += shooterRotation; + + corrected.x = dist * Math.cos(angle); + corrected.y = dist * Math.sin(angle); + + corrected.y += VOPConstants.SHOOTER_CORRECTION; + + return corrected; + } + + /** Corrects odometry guess for gyro angle + * + * @param guess The current guess for the vision center + * @param gyroRotation The rotation to correct for + * @return The corrected odometry point + */ + public static final Point correctGuessForGyro(Point guess, double gyroRotation) { + Point corrected = new Point(guess.x, guess.y); + + double dist = Math.hypot(guess.x, guess.y); + double angle = Math.tan(corrected.y / corrected.x); + angle += gyroRotation; + + corrected.x = dist * Math.cos(angle); + corrected.y = dist * Math.sin(angle); + + return corrected; + } + /** Corrects the angle from the current center estimate to a point on the target rim * for multiple quadrents * diff --git a/src/main/java/frc4388/utility/VisionObscuredException.java b/src/main/java/frc4388/utility/VisionObscuredException.java index 6503b9a..a00a1a7 100644 --- a/src/main/java/frc4388/utility/VisionObscuredException.java +++ b/src/main/java/frc4388/utility/VisionObscuredException.java @@ -4,18 +4,34 @@ package frc4388.utility; * @author Daniel Thomas McGrath */ public class VisionObscuredException extends RuntimeException { + /** + * Creates new VisionObscuredException with error text 'null' + */ public VisionObscuredException() { super(); } + /** Creates new VisionObscuredException with error text message + * + * @param message Error text message + */ public VisionObscuredException(String message) { super(message); } + /** Creates new VisionObscuredException with error text message and detailed stack trace + * + * @param message Error text message + * @param cause Root cause of error + */ public VisionObscuredException(String message, Throwable cause) { super(message, cause); } + /** Creates new VisionObscuredException with error text 'null' and detailed stack trace + * + * @param cause Root cause of error + */ public VisionObscuredException(Throwable cause) { super(cause); }