From d9d0dcc8230cee22fd327a3f7c23c13a5a77cd9d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 8 Apr 2022 12:03:57 -0600 Subject: [PATCH] added trig solution for offset --- src/main/java/frc4388/robot/Constants.java | 3 +++ .../commands/ShooterCommands/TrackTarget.java | 17 +++++++++++------ 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9b2107e..8d03887 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_PIXEL = 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..03f6c8b 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -105,12 +105,8 @@ 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 || @@ -121,9 +117,18 @@ public class TrackTarget extends CommandBase { // RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(), // true); - 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 + pixelOffset) - 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);