From 9d4f9f7020a6b504337728cc0d3edbb74b55ab13 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 7 Mar 2020 11:22:32 -0700 Subject: [PATCH] Lime distance within 5 in Co-Authored-By: Keenan D. Buckley --- src/main/java/frc4388/robot/Constants.java | 4 ++-- .../java/frc4388/robot/commands/shooter/TrackTarget.java | 8 +++++--- 2 files changed, 7 insertions(+), 5 deletions(-) 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