diff --git a/Robot/src/main/java/com/swrobotics/robot/config/NTData.java b/Robot/src/main/java/com/swrobotics/robot/config/NTData.java index b7ab49b..0f0f4d6 100644 --- a/Robot/src/main/java/com/swrobotics/robot/config/NTData.java +++ b/Robot/src/main/java/com/swrobotics/robot/config/NTData.java @@ -92,7 +92,8 @@ public final class NTData { public static final NTEntry SHOOTER_LOB_SHORT_HEIGHT_METERS = new NTDouble("Shooter/Lob/Short Lob Height (m)", 1.3).setPersistent(); public static final NTEntry SHOOTER_LOB_DRIVE_ANGLE_CORRECTION_BLUE = new NTDouble("Shooter/Lob/Drive Angle Correction Blue (deg)", -30).setPersistent(); public static final NTEntry SHOOTER_LOB_DRIVE_ANGLE_CORRECTION_RED = new NTDouble("Shooter/Lob/Drive Angle Correction Red (deg)", 0).setPersistent(); - public static final NTEntry SHOOTER_FLY_TIME = new NTDouble("Shooter/Fly Time (s)", 0.25); + public static final NTEntry SHOOTER_FLY_TIME = new NTDouble("Shooter/Fly Time (s)", 0.25).setPersistent(); + public static final NTEntry GREEN_FN_COEFFICIENT = new NTDouble("Shooter/Green FN Coefficient", 1).setPersistent(); private NTData() { throw new AssertionError(); diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java index bf5cc11..a83eacd 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java @@ -43,7 +43,7 @@ public enum FlywheelControl { private AimCalculator.Aim targetAim; // Target aim is null if not currently aiming private AimCalculator aimCalculator; - private final AimCalculator tableAimCalculator; + private final TableAimCalculator tableAimCalculator; private FlywheelControl flywheelControl; @@ -98,9 +98,21 @@ public void periodic() { aim.pivotAngle()); } else { double distToSpeaker = getSpeakerPosition().getDistance(drive.getEstimatedPose().getTranslation()); - aim = aimCalculator.calculateAim(distToSpeaker); + Pose2d robotPose = drive.getEstimatedPose(); + Translation2d robotPos = robotPose.getTranslation(); + ChassisSpeeds robotSpeeds = drive.getFieldRelativeSpeeds(); + + Translation2d target = getSpeakerPosition(); + Rotation2d angleToTarget = target.minus(robotPos).getAngle(); + + // Relative to the target + Translation2d robotVelocity = new Translation2d(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond).rotateBy(angleToTarget); + aim = tableAimCalculator.calculateAim(distToSpeaker, robotVelocity.getX()); if (RobotBase.isSimulation()) + SimView.lobTrajectory.update( + aim.flywheelVelocity() / NTData.SHOOTER_LOB_POWER_COEFFICIENT.get(), + aim.pivotAngle()); SimView.lobTrajectory.clear(); } targetAim = aim; diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/TableAimCalculator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/TableAimCalculator.java index fa3dbcf..dca655f 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/TableAimCalculator.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/TableAimCalculator.java @@ -102,15 +102,20 @@ private double sample(TreeMap map, double distanceToSpeaker) { @Override public Aim calculateAim(double distanceToSpeaker) { - logDistance.set(distanceToSpeaker); - - distanceToSpeaker *= NTData.SHOOTER_DISTANCE_SCALE.get(); + return calculateAim(distanceToSpeaker, 0.0); + } - return new Aim( - sample(flywheelVelocityMap, distanceToSpeaker), - sample(pivotAngleMap, distanceToSpeaker), - distanceToSpeaker - ); + public Aim calculateAim(double distanceToSpeaker, double velocityTowardsSpeaker) { + logDistance.set(distanceToSpeaker); + System.out.println(velocityTowardsSpeaker); + + distanceToSpeaker *= NTData.SHOOTER_DISTANCE_SCALE.get(); + + return new Aim( + sample(flywheelVelocityMap, distanceToSpeaker), + sample(pivotAngleMap, distanceToSpeaker) + Math.toRadians(NTData.GREEN_FN_COEFFICIENT.get()) * velocityTowardsSpeaker, + distanceToSpeaker + ); } public double getSnapDistance(double currentDist, boolean wantMoveCloser) {