Vision constants

src/main/java/frc/lib/util/AprilTag.java

@@ -5,6 +5,7 @@
 package frc.lib.util;
 
 import java.util.Optional;
+import java.util.OptionalInt;
 
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
@@ -14,6 +15,8 @@
 import edu.wpi.first.networktables.NetworkTableInstance;
 import frc.robot.Constants;
 import frc.robot.RobotContainer;
+import frc.robot.Constants.Limelight;
+import frc.robot.LimelightHelpers;
 import frc.robot.commands.AdvanceToTarget;
 
 // The AprilTag class, harnessed in order to
@@ -179,6 +182,15 @@ public Optional<double[]> determineTargetRotationalOffset(Optional<Translation3d
         return Optional.of(new double[] {xAngularOffset, yAngularOffset});
     }
 
+
+    public Optional<Integer> getTagID() {
+        try {
+            return Optional.of((int) LimelightHelpers.getFiducialID(""));
+        } catch (Exception e) {
+            return Optional.empty();
+        }
+    }
+
     /**
      * Returns the height difference between the robot and the target, if a target
      * is detected.
@@ -191,7 +203,7 @@ public Optional<Double> getHeightDifferenceToTarget() {
       // still need to implement the offsets between the actual target and the
       // apriltag
       if (targetPos().isPresent()) {
-        double heightDifference = targetPos().get().getY();
+        double heightDifference = targetPos().get().getY() - (Constants.Launcher.launcherHeight - Constants.Vision.LimelightOffsetZ) + (Constants.Launcher.speakerHeight - Constants.Limelight.APRILTAGS.get(getTagID().get()).getY() * 0.0254);
         return Optional.of(Math.abs(heightDifference));
       } else {
         return Optional.empty();
@@ -257,4 +269,12 @@ public Optional<Double> getHorizontalVelocity() {
     public double toRPM(double velocity) {
       return velocity * 60 / (2 * Math.PI * Constants.Launcher.launcherWheelRadius);
     }
+
+    public Optional<Double> getLaunchAngle() {
+      if(!targetPos().isPresent()) {
+        return Optional.empty();
+      }
+      return Optional.of(Math.toDegrees(Math.atan(getVerticalVelocity().get() / getHorizontalVelocity().get())) - Constants.Launcher.launcherStartingAngle);
+  }
+
 }

src/main/java/frc/robot/Constants.java

@@ -51,6 +51,18 @@ public static final class PathPlanner {
     public static final double turnKI = 0.0;
     public static final double turnKD = 0.75;
   }
+  public static final class Launcher {
+    public static final double launcherWheelRadius = 0.1016;
+    public static final double gravityAcceleration = 9.81;
+    public static final double kP = 0.05;
+    public static final double kI = 0.0001;
+    public static final double kD = 0.0;
+    public static final double ampHeight = 0.66;
+    public static final double speakerHeight = 1.984;
+    public static final double trapHeight = 1.569;
+    public static final double launcherHeight = 0.584;
+    public static final double launcherStartingAngle = 25;
+    public static final double feedVelocity = 0.5; // just a placeholder value - will need to be changed
 
   public static final class Intake {
     public static final int intakeMotorPWMPort = 0;
@@ -65,7 +77,7 @@ public static final class Vision {
     //all units in meters
     // sign of offsets follows std convention rel to swerve
     public static final double LimelightAngleDegrees = 30;
-    public static final double LimelightOffsetZ = 10.5 * 0.0254;
+    public static final double LimelightOffsetZ = 0.2667;
     public static final double LimelightOffsetX = 0.32;
     public static final double LimelightOffsetY = 0.0;