diff --git a/Robot/src/main/java/com/swrobotics/robot/commands/AimTowardsLobCommand.java b/Robot/src/main/java/com/swrobotics/robot/commands/AimTowardsLobCommand.java index 3371814..d527eb8 100644 --- a/Robot/src/main/java/com/swrobotics/robot/commands/AimTowardsLobCommand.java +++ b/Robot/src/main/java/com/swrobotics/robot/commands/AimTowardsLobCommand.java @@ -1,7 +1,5 @@ package com.swrobotics.robot.commands; -import org.littletonrobotics.junction.AutoLogOutput; - import com.swrobotics.lib.net.NTUtil; import com.swrobotics.mathlib.MathUtil; import com.swrobotics.robot.config.NTData; @@ -12,10 +10,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.proto.ChassisSpeedsProto; -import edu.wpi.first.math.kinematics.struct.ChassisSpeedsStruct; import edu.wpi.first.wpilibj2.command.Command; public final class AimTowardsLobCommand extends Command { 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 0704e7b..4d93927 100644 --- a/Robot/src/main/java/com/swrobotics/robot/config/NTData.java +++ b/Robot/src/main/java/com/swrobotics/robot/config/NTData.java @@ -117,7 +117,8 @@ public final class NTData { public static final NTEntry SHOOTER_PIVOT_REVERSE_ANGLE = new NTDouble("Shooter/Pivot/Reverse Angle (deg)", 35).setPersistent(); public static final NTEntry SHOOTER_LOB_POWER_COEFFICIENT = new NTDouble("Shooter/Lob/Power Coefficient", 1.25).setPersistent(); // To go from real velocity to flywheel velocity - public static final NTEntry SHOOTER_LOB_HEIGHT_METERS = new NTDouble("Shooter/Lob/Lob Height (m)", 3).setPersistent(); + public static final NTEntry SHOOTER_LOB_TALL_HEIGHT_METERS = new NTDouble("Shooter/Lob/Tall Lob Height (m)", 4).setPersistent(); + public static final NTEntry SHOOTER_LOB_SHORT_HEIGHT_METERS = new NTDouble("Shooter/Lob/Short Lob Height (m)", 1).setPersistent(); public static final NTEntry SHOOTER_FLY_TIME = new NTDouble("Shooter/Fly Time (s)", 0.25); private NTData() { diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/LobCalculator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/LobCalculator.java index 06eeac9..af09868 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/LobCalculator.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/LobCalculator.java @@ -10,15 +10,26 @@ public class LobCalculator implements AimCalculator { public static final LobCalculator INSTANCE = new LobCalculator(); private static final double twoG = 9.8 * 2; - private double sqrt2gh; + private double tallSqrt2gh; + private double shortSqrt2gh; public LobCalculator() { - NTData.SHOOTER_LOB_HEIGHT_METERS.onChange((a) -> this.updateHeight()); - sqrt2gh = Math.sqrt(twoG * NTData.SHOOTER_LOB_HEIGHT_METERS.get()); + NTData.SHOOTER_LOB_TALL_HEIGHT_METERS.onChange((a) -> this.updateHeight()); + NTData.SHOOTER_LOB_SHORT_HEIGHT_METERS.onChange((a) -> this.updateHeight()); + tallSqrt2gh = Math.sqrt(twoG * NTData.SHOOTER_LOB_TALL_HEIGHT_METERS.get()); + shortSqrt2gh = Math.sqrt(twoG * NTData.SHOOTER_LOB_SHORT_HEIGHT_METERS.get()); } public Aim calculateAim(double distanceToSpeaker, double velocityTowardsGoal) { - double angleRad = Math.atan2(4 * NTData.SHOOTER_LOB_HEIGHT_METERS.get(), distanceToSpeaker); + boolean inWing = distanceToSpeaker > 5; + double angleRad = 0; + + if (inWing) { + angleRad = Math.atan2(4 * NTData.SHOOTER_LOB_TALL_HEIGHT_METERS.get(), distanceToSpeaker); + } + + double sqrt2gh = (inWing) ? tallSqrt2gh : shortSqrt2gh; + double velocity = sqrt2gh / Math.sin(angleRad); Translation2d velocityVector = new Translation2d(velocity, new Rotation2d(angleRad)); @@ -37,7 +48,8 @@ public Aim calculateAim(double distanceToSpeaker) { } private void updateHeight() { - sqrt2gh = Math.sqrt(twoG * NTData.SHOOTER_LOB_HEIGHT_METERS.get()); + tallSqrt2gh = Math.sqrt(twoG * NTData.SHOOTER_LOB_TALL_HEIGHT_METERS.get()); + shortSqrt2gh = Math.sqrt(twoG * NTData.SHOOTER_LOB_SHORT_HEIGHT_METERS.get()); } }