diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 221430e..e9ac88d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -198,8 +198,8 @@ public final class Constants { public static final class VisionConstants { public static final double FOV = 29.8; //Field of view of limelight - public static final double TARGET_HEIGHT = 71; - public static final double LIME_ANGLE = 25; + public static final double TARGET_HEIGHT = 66.75; + public static final double LIME_ANGLE = 28.387; public static final double TURN_P_VALUE = 0.6; public static final double X_ANGLE_ERROR = 1.3; public static final double MOTOR_DEAD_ZONE = 0.3; diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index 16b770a..d4bb084 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -32,6 +32,7 @@ public class TrackTarget extends CommandBase { double yAngle = 0; double target = 0; public double distance; + public double realDistance; public static double fireVel; public static double fireAngle; @@ -83,7 +84,8 @@ public class TrackTarget extends CommandBase { // Finding Distance distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); - SmartDashboard.putNumber("Distance to Target", distance); + realDistance = (1.13 * distance) - 14.3; + SmartDashboard.putNumber("Distance to Target", realDistance); SmartDashboard.putNumber("ts Skew or Rotation", NetworkTableInstance.getDefault().getTable("limelight").getEntry("ts").getDouble(0)); SmartDashboard.putNumber("ta Area", NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0)); //START Equation Code @@ -95,8 +97,8 @@ public class TrackTarget extends CommandBase { //END Equation Code //START CSV Code - fireVel = m_shooter.m_shooterTable.getVelocity(distance); - fireAngle = m_shooter.m_shooterTable.getHood(distance); //Note: Ensure to follow because units are different + fireVel = m_shooter.m_shooterTable.getVelocity(realDistance); + fireAngle = m_shooter.m_shooterTable.getHood(realDistance); //Note: Ensure to follow because units are different //fireAngle = 33; //END CSV Code