sim testing

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

@@ -27,6 +27,7 @@
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.ShooterIOSim;
 import frc.robot.subsystems.shooter.ShooterIOTalonFX;
+import frc.robot.subsystems.sotm.ShotCalculator;
 import frc.robot.subsystems.swerve.CommandSwerveDrivetrain;
 import frc.robot.subsystems.swerve.generated.TunerConstants;
 import frc.robot.subsystems.turret.Turret;
@@ -65,6 +66,8 @@ public class RobotContainer {
   private final Turret turret =
       new Turret(true, Utils.isSimulation() ? new TurretIOSim() : new TurretIOTalonFX());
 
+  private final ShotCalculator shotCalculator = new ShotCalculator(drivetrain);
+
   /// sim file for intakepivot needs to be added -- seems like its not been merged yet
 
   private AutoChooser autoChooser = new AutoChooser();
@@ -88,7 +91,7 @@ private void configureOperatorBinds() {
 
     m_operatorController.a().whileTrue(shooter.setVoltage(12));
     m_operatorController.b().whileTrue(indexer.setVoltage(12));
-    m_operatorController.x().onTrue(turret.pointToPose(()->drivetrain.getState().Pose, ()-> new Pose2d(FieldConstants.Hub.innerCenterPoint.toTranslation2d(), Rotation2d.kZero)));
+    m_operatorController.x().onTrue(turret.pointToPose(()->drivetrain.getState().Pose, ()->shotCalculator.getCurrentEffectiveTargetPose().toPose2d()));
   }
 
   private void configureChoreoAutoChooser() {
@@ -145,5 +148,6 @@ private void configureSwerve() {
   }
 
   public void periodic() {
+    shotCalculator.periodic();
   }
 }

src/main/java/frc/robot/subsystems/Superstructure.java

@@ -19,6 +19,7 @@
 import frc.robot.subsystems.intakerollers.IntakeRollers;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooterpivot.ShooterPivot;
+import frc.robot.subsystems.sotm.ShotCalculator;
 import frc.robot.subsystems.turret.Turret;
 import frc.robot.utils.LoggedTracer;
 import java.util.HashMap;
@@ -55,6 +56,8 @@ public enum StructureState {
   private final Feeder feeder;
   private final Turret turret;
 
+  private final ShotCalculator shotCalculator;
+
   private final Supplier<Pose2d> robotPoseSupplier;
 
   public Superstructure(
@@ -66,7 +69,7 @@ public Superstructure(
       Climb climb,
       Feeder feeder,
       Turret turret,
-      //  ShotCalculator shotCalculator;
+        ShotCalculator shotCalculator,
       Supplier<Pose2d> robotPoseSupplier) {
     this.indexer = indexer;
     this.shooterPivot = shooterPivot;
@@ -76,7 +79,7 @@ public Superstructure(
     this.climb = climb;
     this.feeder = feeder;
     this.turret = turret;
-    // this.shotCalculator = shotCalculator;
+     this.shotCalculator = shotCalculator;
     this.robotPoseSupplier = robotPoseSupplier;
 
     // intake --> indexer --> feeder --> shooter
@@ -137,6 +140,7 @@ public void periodic() {
     Logger.recordOutput("Superstructure/StateTime", this.stateTimer.get());
 
     LoggedTracer.record(this.getClass().getSimpleName());
+
   }
 
   public Command setState(StructureState state) {

src/main/java/frc/robot/subsystems/sotm/ShotCalculator.java

@@ -17,6 +17,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.FieldConstants;
 import frc.robot.subsystems.swerve.CommandSwerveDrivetrain;
+import frc.robot.utils.LoggedTracer;
 import frc.robot.utils.sotm.ChassisAccelerations;
 import frc.robot.utils.sotm.ShootOnTheFlyCalculator;
 import frc.robot.utils.sotm.ShootOnTheFlyCalculator.InterceptSolution;
@@ -42,12 +43,15 @@ public ShotCalculator(CommandSwerveDrivetrain drivetrain) {
   public void periodic() {
     Pose2d drivetrainPose = drivetrain.getState().Pose;
 
+
     targetDistance =
         drivetrainPose.getTranslation().getDistance(targetLocation.toPose2d().getTranslation());
     targetSpeedRps = DISTANCE_TO_SHOOTER_SPEED.get(targetDistance);
 
+
     Pose3d shooterPose = new Pose3d(drivetrainPose).plus(ROBOT_TO_SHOOTER);
 
+
     ChassisSpeeds drivetrainSpeeds = drivetrain.getState().Speeds;
     ChassisAccelerations drivetrainAccelerations = drivetrain.getFieldRelativeAccelerations();
 
@@ -69,6 +73,8 @@ public void periodic() {
     Logger.recordOutput("ShotCalculator/TargetDistance", targetDistance);
     Logger.recordOutput("ShotCalculator/TargetLocation", targetLocation);
     Logger.recordOutput("ShotCalculator/TargetSpeedRps", targetSpeedRps);
+
+    LoggedTracer.record("ShotCalculator");
   }
 
   public void setTarget(Pose3d targetLocation, double targetSpeedRps) {

src/main/java/frc/robot/subsystems/sotm/ShotCalculatorConstants.java

@@ -14,13 +14,8 @@
 
 public final class ShotCalculatorConstants {
 
-  public static final double kGravity = 9.81; // m/s^2
-
-  public static final int kMaxIterations = 10;
-  public static final double kConvergenceThreshold = 0.01; // meters
-
   public static final Transform3d ROBOT_TO_SHOOTER =
-      new Transform3d(new Translation3d(0.0, 0.0, 0.3), new Rotation3d());
+      new Transform3d(new Translation3d(-0.24, 0.0, 0.5), Rotation3d.kZero);
 
   // distance to shooter speed
   // meters to RPS
@@ -36,17 +31,16 @@ public final class ShotCalculatorConstants {
 
     // distance to shooter speed
     // meters to RPS
-    DISTANCE_TO_SHOOTER_SPEED.put(1.0, 40.0);
+    DISTANCE_TO_SHOOTER_SPEED.put(2.07, 7.0);
+    DISTANCE_TO_SHOOTER_SPEED.put(4.92, 9.0);
+
 
     // distance to pivot angle
     // meters to radians
     DISTANCE_TO_PIVOT_ANGLE.put(1.0, Math.toRadians(45));
+    DISTANCE_TO_PIVOT_ANGLE.put(2.0, Math.toRadians(45));
+    DISTANCE_TO_PIVOT_ANGLE.put(3.0, Math.toRadians(45));
+    DISTANCE_TO_PIVOT_ANGLE.put(4.0, Math.toRadians(45));
   }
 
-  public static final double DEFAULT_PROJECTILE_VELOCITY = 10.0; // m/s
-
-  public static final double MAX_SHOT_DISTANCE = 8.0; // meters
-  public static final double MIN_SHOT_DISTANCE = 0.5; // meters
-  public static final double MAX_PIVOT_ANGLE = Math.toRadians(60); // radians
-  public static final double MIN_PIVOT_ANGLE = Math.toRadians(10); // radians
 }

src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java

@@ -39,6 +39,8 @@
 import frc.robot.utils.LoggedTracer;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
+
+import frc.robot.utils.sotm.ChassisAccelerations;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
@@ -262,15 +264,11 @@ public void periodic() {
               });
     }
 
-    double currentTime = Logger.getTimestamp() / 1e6;
-    ChassisSpeeds currentSpeeds = getState().Speeds;
 
-    if (previousTime > 0) {
-      double dt = currentTime - previousTime;
-    }
 
-    previousSpeeds = currentSpeeds;
-    previousTime = currentTime;
+
+    previousSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(getState().Speeds, getState().RawHeading);
+    previousTime = getState().Timestamp;
 
     LoggedTracer.record(this.getClass().getSimpleName());
   }
@@ -287,13 +285,13 @@ public void addPhotonEstimate(
     // }
   }
 
-  public frc.robot.utils.sotm.ChassisAccelerations getFieldRelativeAccelerations() {
-    double currentTime = Logger.getTimestamp() / 1e6;
-    double dt = previousTime > 0 ? currentTime - previousTime : 0.02;
+  public ChassisAccelerations getFieldRelativeAccelerations() {
 
-    ChassisSpeeds currentSpeeds = getState().Speeds;
+    return new ChassisAccelerations(getFieldRelativeSpeeds(), previousSpeeds, getState().Timestamp-previousTime);
+  }
 
-    return new frc.robot.utils.sotm.ChassisAccelerations(currentSpeeds, previousSpeeds, dt);
+  public ChassisSpeeds getFieldRelativeSpeeds() {
+    return ChassisSpeeds.fromRobotRelativeSpeeds(getState().Speeds, getState().RawHeading);
   }
 
   /**

src/main/java/frc/robot/utils/sotm/BallPhysics.java

@@ -15,28 +15,37 @@
 public final class BallPhysics {
   public static final double GRAVITY = 9.81;
 
-  public record ShotSolution(double launchPitchRad, double launchSpeed, double flightTimeSeconds) {}
+  public record ShotSolution(
+          double launchPitchRad,
+          double launchSpeed,
+          double flightTimeSeconds) {
+  }
 
-  private BallPhysics() {}
+  private BallPhysics() {
+  }
 
   private static Translation3d gravityForce(BallConstants c) {
     return new Translation3d(0, 0, -c.mass * c.gravity);
   }
 
-  private static Translation3d dragForce(Translation3d v, BallConstants c) {
+  private static Translation3d dragForce(
+          Translation3d v, BallConstants c) {
 
     double speed = v.getNorm();
-    if (speed < 1e-6) return new Translation3d();
+    if (speed < 1e-6)
+      return new Translation3d();
 
     double scale = -0.5 * c.rho * c.cd * c.area * speed;
     return v.times(scale);
   }
 
-  private static Translation3d magnusForce(Translation3d v, Translation3d omega, BallConstants c) {
+  private static Translation3d magnusForce(
+          Translation3d v, Translation3d omega, BallConstants c) {
 
     double speed = v.getNorm();
     double wMag = omega.getNorm();
-    if (speed < 1e-6 || wMag < 1e-6) return new Translation3d();
+    if (speed < 1e-6 || wMag < 1e-6)
+      return new Translation3d();
 
     double spinRatio = wMag * c.radius / speed;
     double cl = Math.min(c.clGain * spinRatio, c.clMax);
@@ -50,17 +59,25 @@ private static Translation3d magnusForce(Translation3d v, Translation3d omega, B
     return direction.times(magnitude);
   }
 
-  private static Rotation3d integrateRotation(Rotation3d current, Translation3d omega, double dt) {
+  private static Rotation3d integrateRotation(
+          Rotation3d current,
+          Translation3d omega,
+          double dt) {
 
-    Rotation3d delta = new Rotation3d(omega.getX() * dt, omega.getY() * dt, omega.getZ() * dt);
+    Rotation3d delta = new Rotation3d(
+            omega.getX() * dt,
+            omega.getY() * dt,
+            omega.getZ() * dt);
 
     return current.plus(delta);
   }
 
-  public static void step(BallState s, BallConstants c, double dt) {
+  public static void step(
+          BallState s, BallConstants c, double dt) {
 
-    Translation3d force =
-        gravityForce(c).plus(dragForce(s.velocity, c)).plus(magnusForce(s.velocity, s.omega, c));
+    Translation3d force = gravityForce(c)
+            .plus(dragForce(s.velocity, c))
+            .plus(magnusForce(s.velocity, s.omega, c));
 
     Translation3d accel = force.div(c.mass);
 
@@ -69,14 +86,14 @@ public static void step(BallState s, BallConstants c, double dt) {
 
     s.velocity = s.velocity.plus(accel.times(dt));
 
-    s.pose =
-        new Pose3d(
-            s.pose.getTranslation().plus(s.velocity.times(dt)),
+    s.pose = new Pose3d(s.pose.getTranslation().plus(s.velocity.times(dt)),
             integrateRotation(s.pose.getRotation(), s.omega, dt));
   }
 
   public static ShotSolution solveBallisticWithIncomingAngle(
-      Pose3d shooterPose, Pose3d targetPose, double incomingPitchRad) {
+          Pose3d shooterPose,
+          Pose3d targetPose,
+          double incomingPitchRad) {
 
     Translation3d s = shooterPose.getTranslation();
     Translation3d t = targetPose.getTranslation();
@@ -94,7 +111,8 @@ public static ShotSolution solveBallisticWithIncomingAngle(
 
     double rhs = dz - d * tanThetaT;
     if (rhs <= 0) {
-      throw new IllegalArgumentException("No physical solution: dz - d*tan(thetaT) must be > 0");
+      throw new IllegalArgumentException(
+              "No physical solution: dz - d*tan(thetaT) must be > 0");
     }
 
     double T = Math.sqrt(2.0 * rhs / GRAVITY);
@@ -109,7 +127,9 @@ public static ShotSolution solveBallisticWithIncomingAngle(
   }
 
   public static ShotSolution solveBallisticWithSpeed(
-      Pose3d shooterPose, Pose3d targetPose, double launchSpeed) {
+          Pose3d shooterPose,
+          Pose3d targetPose,
+          double launchSpeed) {
 
     Translation3d s = shooterPose.getTranslation();
     Translation3d t = targetPose.getTranslation();
@@ -142,7 +162,9 @@ public static ShotSolution solveBallisticWithSpeed(
     return new ShotSolution(launchPitch, launchSpeed, time);
   }
 
-  public static double minSpeedForAnyArc(Pose3d shooterPose, Pose3d targetPose) {
+  public static double minSpeedForAnyArc(
+          Pose3d shooterPose,
+          Pose3d targetPose) {
 
     Translation3d s = shooterPose.getTranslation();
     Translation3d t = targetPose.getTranslation();
@@ -153,6 +175,7 @@ public static double minSpeedForAnyArc(Pose3d shooterPose, Pose3d targetPose) {
 
     double d = Math.hypot(dx, dy);
 
-    return Math.sqrt(GRAVITY * (Math.hypot(d, dz) + dz));
+    return Math.sqrt(
+            GRAVITY * (Math.hypot(d, dz) + dz));
   }
 }