142 calibration of speeds inside of sim

src/main/java/frc/robot/RobotContainer.java

@@ -179,7 +179,7 @@ private void configureDriverBindings(PatriBoxController controller) {
             .onTrue(intake.outCommand());
 
         controller.leftBumper()
-            .toggleOnTrue(shooterCalc.prepareFireViaMathCommand(() -> true, swerve::getPose, swerve::getRobotRelativeVelocity));
+            .toggleOnTrue(shooterCalc.prepareSWDSimCommand(() -> true, swerve::getPose, swerve::getRobotRelativeVelocity));
 
         controller.leftTrigger()
             .onTrue(shooterCalc.resetShooter());

src/main/java/frc/robot/commands/NoteTrajectory.java

@@ -18,6 +18,7 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.RobotContainer;
+import frc.robot.util.Constants;
 import frc.robot.util.Constants.NTConstants;
 
 public class NoteTrajectory extends Command {
@@ -60,12 +61,14 @@ public void initialize() {
         x0 = NTConstants.PIVOT_OFFSET_METERS.getX();
         y0 = 0;
         z0 = NTConstants.PIVOT_OFFSET_METERS.getY();
+
         vx0 = Rotation2d.fromDegrees(pivotAngle).getCos() * initialVelocity + initalSpeeds.vxMetersPerSecond;
         vy0 = initalSpeeds.vyMetersPerSecond;
         vz0 = Rotation2d.fromDegrees(pivotAngle).getSin() * initialVelocity;
+
         ax = 0;
         ay = 0;
-        az = -9.8;
+        az = -Constants.GRAVITY;
     }
 
     // Called every time the scheduler runs while the command is scheduled.

src/main/java/frc/robot/commands/ShooterCalc.java

@@ -18,6 +18,7 @@
 import frc.robot.util.Constants.ShooterConstants;
 import monologue.Logged;
 import monologue.Annotations.Log;
+import frc.robot.util.Constants;
 import frc.robot.util.SpeedAngleTriplet;
 
 public class ShooterCalc implements Logged {
@@ -76,15 +77,15 @@ public Command prepareFireCommand(BooleanSupplier shootAtSpeaker, Supplier<Pose2
      *                       of the robot. It is of type `Supplier<Pose2d>`.
      * @return The method is returning a Command object.
      */
-    public Command prepareFireMovingCommand(Supplier<Pose2d> robotPose, Supplier<ChassisSpeeds> speeds) {
+    public Command prepareSWDCommand(Supplier<Pose2d> robotPose, Supplier<ChassisSpeeds> speeds) {
         return Commands.run(() -> {
             SpeedAngleTriplet triplet = calculateSWDTriplet(robotPose.get(), speeds.get());
             pivot.setAngle(triplet.getAngle());
             shooter.setSpeed(triplet.getSpeeds());
         }, pivot, shooter);
     }
 
-    public Command prepareFireViaMathCommand(BooleanSupplier shootAtSpeaker, Supplier<Pose2d> robotPose, Supplier<ChassisSpeeds> speeds) {
+    public Command prepareSWDSimCommand(BooleanSupplier shootAtSpeaker, Supplier<Pose2d> robotPose, Supplier<ChassisSpeeds> robotSpeeds) {
         return Commands.run(() -> {
             Rotation2d angle = calculatePivotAngle(robotPose.get());
             Pair<Double, Double> speedsPair = calculateShooterSpeedsForApex(robotPose.get(), angle);
@@ -332,33 +333,14 @@ public double velocityToRPM(double noteVelocity) {
      * @return              a pair of shooter speeds (left and right) required to reach the speaker position
      */
     private Pair<Double, Double> calculateShooterSpeedsForApex(Pose2d robotPose, Rotation2d pivotAngle) {
-        double desiredRPM = velocityToRPM(Math.sqrt(2 * 9.8 * 2.08) / (pivotAngle.getSin()));
+        double desiredRPM = velocityToRPM(Math.sqrt(2 * Constants.GRAVITY * 2.08) / (pivotAngle.getSin()));
         return Pair.of(desiredRPM, desiredRPM);
     }
 
     public Command getNoteTrajectoryCommand(Supplier<Pose2d> pose, Supplier<ChassisSpeeds> speeds) {
         return Commands.runOnce(
             () -> {
-                Pose2d currentPose = pose.get();
-                Rotation2d pivotAngle = calculatePivotAngle(currentPose);
-                SpeedAngleTriplet currentTriplet = SpeedAngleTriplet.of(calculateShooterSpeedsForApex(currentPose, pivotAngle), pivotAngle.getDegrees());
-                double normalVelocity = -getVelocityVectorToSpeaker(currentPose, speeds.get()).getY();
-
-                double originalv0 = rpmToVelocity(currentTriplet.getSpeeds());
-                double v0z = Math.sqrt(9.8*2*2.08);
-                double v0x = originalv0 * Math.cos(Math.toRadians(currentTriplet.getAngle())) + normalVelocity;
-
-                double newv0 = Math.sqrt(Math.pow(v0x,2)+ Math.pow(v0z,2));
-                Rotation2d newAngle = new Rotation2d(v0x, v0z);
-
-                SpeedAngleTriplet calculationTriplet = 
-                    SpeedAngleTriplet.of(
-                        Pair.of(
-                            velocityToRPM(newv0),
-                            velocityToRPM(newv0)
-                        ),
-                        newAngle.getDegrees()
-                    );
+                SpeedAngleTriplet calculationTriplet = calculateSWDTriplet(pose.get(), speeds.get());
 
                 new NoteTrajectory(
                     pose,
@@ -384,10 +366,10 @@ private SpeedAngleTriplet calculateSWDTriplet(Pose2d pose, ChassisSpeeds speeds)
         double normalVelocity = -getVelocityVectorToSpeaker(currentPose, speeds).getY();
 
         double originalv0 = rpmToVelocity(currentTriplet.getSpeeds());
-        double v0z = Math.sqrt(9.8*2*2.08);
-        double v0x = originalv0 * Math.cos(Math.toRadians(currentTriplet.getAngle())) + normalVelocity;
+        double v0z = Math.sqrt(Constants.GRAVITY*2*2.08);
+        double v0x = originalv0 * Math.cos(Units.degreesToRadians(currentTriplet.getAngle())) + normalVelocity;
 
-        double newv0 = Math.sqrt(Math.pow(v0x,2)+ Math.pow(v0z,2));
+        double newv0 = Math.hypot(v0x, v0z);
         Rotation2d newAngle = new Rotation2d(v0x, v0z);
 
         return 

src/main/java/frc/robot/util/Constants.java

@@ -504,7 +504,7 @@ public static enum GameMode {
                 // Blue Speaker
                 new Pose2d(0, 5.547, Rotation2d.fromDegrees(0)),
                 // Red Speaker
-                new Pose2d(FIELD_WIDTH_METERS, 5.547, Rotation2d.fromDegrees(180)),
+                new Pose2d(FIELD_WIDTH_METERS, 5.547, Rotation2d.fromDegrees(0)),
         };
 
         private static final Pose2d[] AMP_POSITIONS = new Pose2d[] {
@@ -543,4 +543,6 @@ public static final class NTConstants {
         public static final Translation2d PIVOT_OFFSET_METERS = new Translation2d(0.112, 0.21);
     }
 
+    public static final double GRAVITY = 9.8;
+
 }