From 3b5749569bb6bec51dfbeb720cd436f6af060045 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 8 Apr 2022 12:33:42 -0600 Subject: [PATCH] trig solution --- src/main/java/frc4388/robot/Constants.java | 3 +++ .../commands/ShooterCommands/TrackTarget.java | 15 ++++++++++----- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 47b9ab5..a68b9ac 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -341,5 +341,8 @@ public final class Constants { public static final double EDGE_TO_CENTER = 20; public static final double LIMELIGHT_RADIUS = 8; public static final double SHOOTER_CORRECTION = 1.d; + + public static final double PIXELS_PER_DEGREE = LIME_HIXELS / H_FOV; + public static final double DEGREES_PER_PIXELS = 1 / PIXELS_PER_DEGREE; } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 9151974..608060c 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -106,11 +106,6 @@ public class TrackTarget extends CommandBase { // points = getFakePoints(); //// points = filterPoints(points); Point average = VisionOdometry.averagePoint(points); - - double output = ((average.x + 40) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; - output *= 2.1; - - m_turret.runTurretWithInput(output); // double position = m_turret.m_boomBoomRotateEncoder.getPosition(); // if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 || @@ -123,7 +118,17 @@ public class TrackTarget extends CommandBase { double regressedDistance = getDistance(average.y); + + // ! offset trig solution + double desiredOffset = 10; // * inches + double angleOffset = Math.toDegrees(Math.atan(desiredOffset / regressedDistance)); // * degrees + double pixelOffset = angleOffset * VisionConstants.PIXELS_PER_DEGREE; + + double output = ((average.x + 40) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; + output *= 2.1; + m_turret.runTurretWithInput(output); + // ! no longer a +30 lol -aarav double distAdj = SmartDashboard.getNumber("Distance Adjust", -35); velocity = m_boomBoom.getVelocity(regressedDistance + distAdj);