Debug/launch target velocity

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

@@ -4,16 +4,13 @@
 
 package frc.robot;
 
-import static edu.wpi.first.units.Units.RadiansPerSecond;
-
 import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -310,15 +307,24 @@ public Command getAutonomousCommand() {
   private SwerveRequest.FieldCentric getDriveAndLaunchRequest() {
     LaunchRequest launchRequest = launchState.getLaunchRequest();
     double rotationalRate =
-        launchRequest.getTargetRobotAngularVelocity().in(RadiansPerSecond)
-            + DrivePreferences.autoAim_kP.getValue()
+        // kP * error
+        DrivePreferences.autoAim_kP.getValue()
                 * launchRequest
                     .getTargetRobotAngle()
                     .minus(driveState.getCurrentDriveStats().Pose.getRotation())
                     .getRadians()
+            // kD * rate of error ((target - currentAngle - (target - previousAngle))/period =
+            // (previousAngle - currentAngle)/period)
             + DrivePreferences.autoAim_kD.getValue()
-                * (launchRequest.getTargetRobotAngularVelocity().in(RadiansPerSecond)
-                    - driveState.getFieldVelocity().omegaRadiansPerSecond);
+                * driveState
+                    .getPreviousDriveStats()
+                    .Pose
+                    .getRotation()
+                    .minus(driveState.getCurrentDriveStats().Pose.getRotation())
+                    .getRadians()
+                / (driveState.getCurrentDriveStats().Timestamp
+                    - driveState.getPreviousDriveStats().Timestamp);
+
     return DriveConstants.DEFAULT_DRIVE_REQUEST
         .withVelocityX(
             -1
@@ -339,19 +345,21 @@ private SwerveRequest.RobotCentric getDriveToLemonRequest() {
             .getTranslation()
             .minus(driveState.getCurrentDriveStats().Pose.getTranslation())
             .getAngle();
-    AngularVelocity targetAngularVelocity =
-        RadiansPerSecond.of(
-            targetAngle.minus(driveState.getPreviousDriveStats().Pose.getRotation()).getRadians());
 
     double rotationalRate =
-        targetAngularVelocity.in(RadiansPerSecond)
-            + DrivePreferences.autoAim_kP.getValue()
+        DrivePreferences.autoAim_kP.getValue()
                 * targetAngle
                     .minus(driveState.getCurrentDriveStats().Pose.getRotation())
                     .getRadians()
             + DrivePreferences.autoAim_kD.getValue()
-                * (targetAngularVelocity.in(RadiansPerSecond)
-                    - driveState.getFieldVelocity().omegaRadiansPerSecond);
+                * driveState
+                    .getPreviousDriveStats()
+                    .Pose
+                    .getRotation()
+                    .minus(driveState.getCurrentDriveStats().Pose.getRotation())
+                    .getRadians()
+                / (driveState.getCurrentDriveStats().Timestamp
+                    - driveState.getPreviousDriveStats().Timestamp);
 
     return DriveConstants.LEMON_HUNTING_REQUEST
         .withVelocityX(DriveConstants.HUNT_SPEED) // Drive forward with negative Y (forward)

src/main/java/frc/robot/statemachines/LaunchCalculator.java

@@ -1,17 +1,14 @@
 package frc.robot.statemachines;
 
 import static edu.wpi.first.units.Units.Meters;
-import static edu.wpi.first.units.Units.RadiansPerSecond;
 
-import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 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.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.statemachines.LaunchState.LaunchType;
 import frc.robot.subsystems.shooter.LaunchRequest;
@@ -29,11 +26,6 @@ protected static synchronized LaunchCalculator getInstance() {
     return single_instance;
   }
 
-  private double loopPeriodSecs = 0.02;
-
-  private final LinearFilter driveAngleFilter =
-      LinearFilter.movingAverage((int) (0.8 / loopPeriodSecs));
-
   private static final double phaseDelay;
 
   private static final InterpolatingDoubleTreeMap hubTimeOfFlightMap =
@@ -115,53 +107,19 @@ protected LaunchRequest refreshRequest(Pose3d target, LaunchType builderType) {
     Rotation2d targetRobotAngle =
         target.getTranslation().toTranslation2d().minus(lookaheadPose.getTranslation()).getAngle();
 
-    // Rotation2d targetRobotAngle = getDriveAngle(lookaheadPose,
-    // target.getTranslation().toTranslation2d());
-
-    AngularVelocity targetRobotAngularVelocity =
-        // RadiansPerSecond.of(
-        //     driveAngleFilter.calculate(
-        //         targetRobotAngle
-        //
-        // .minus(DriveState.getInstance().getPreviousDriveStats().Pose.getRotation())
-        //                 .getRadians()
-        //             / loopPeriodSecs));
-        RadiansPerSecond.of(
-            targetRobotAngle
-                .minus(DriveState.getInstance().getPreviousDriveStats().Pose.getRotation())
-                .getRadians());
-
     if (builderType == LaunchType.MAPPED)
       return new MappedLaunchRequestBuilder()
           .createLaunchRequest(
               passing,
               lookaheadLauncherToTargetDistance,
-              targetRobotAngularVelocity,
               targetRobotAngle,
               Meters.of(launcherToTargetDistance));
     else
       return new ParabolicLaunchRequestBuilder()
           .createLaunchRequest(
               passing,
               lookaheadLauncherToTargetDistance,
-              targetRobotAngularVelocity,
               targetRobotAngle,
               Meters.of(launcherToTargetDistance));
   }
-
-  /*
-  private static Rotation2d getDriveAngle(Pose2d robotPose, Translation2d target) {
-    Rotation2d fieldToHubAngle = target.minus(robotPose.getTranslation()).getAngle();
-    Rotation2d hubAngle =
-        new Rotation2d(
-            Math.asin(
-                MathUtil.clamp(
-                    robotPose.getTranslation().getY()
-                        / target.getDistance(robotPose.getTranslation()),
-                    -1.0,
-                    1.0)));
-    Rotation2d driveAngle = fieldToHubAngle.plus(hubAngle).plus(robotPose.getRotation());
-    return driveAngle;
-  }
-  */
 }

src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java

@@ -14,9 +14,6 @@ public class LaunchRequest {
   @Logged(name = "Launch Flywheel Velocity", importance = Logged.Importance.CRITICAL)
   private AngularVelocity launchVelocity;
 
-  @Logged(name = "Target Robot Angular Velocity", importance = Logged.Importance.CRITICAL)
-  private AngularVelocity targetRobotAngularVelocity;
-
   @Logged(name = "Target Robot Angle", importance = Logged.Importance.CRITICAL)
   private Rotation2d targetRobotAngle;
 
@@ -28,13 +25,11 @@ public class LaunchRequest {
   public LaunchRequest(
       Angle launchHoodTarget,
       AngularVelocity launchVelocity,
-      AngularVelocity targetRobotAngularVelocity,
       Rotation2d targetRobotAngle,
       Distance targetDistance,
       double timestamp) {
     this.launchHoodTarget = launchHoodTarget;
     this.launchVelocity = launchVelocity;
-    this.targetRobotAngularVelocity = targetRobotAngularVelocity;
     this.targetRobotAngle = targetRobotAngle;
     this.targetDistance = targetDistance;
     this.timestamp = timestamp;
@@ -48,10 +43,6 @@ public AngularVelocity getFlywheelVelocity() {
     return launchVelocity;
   }
 
-  public AngularVelocity getTargetRobotAngularVelocity() {
-    return targetRobotAngularVelocity;
-  }
-
   public Rotation2d getTargetRobotAngle() {
     return targetRobotAngle;
   }

src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java

@@ -1,14 +1,9 @@
 package frc.robot.subsystems.shooter;
 
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
 
 public interface LaunchRequestBuilder {
   public LaunchRequest createLaunchRequest(
-      boolean passing,
-      double distance,
-      AngularVelocity targetRobotAngularVelocity,
-      Rotation2d targetRobotAngle,
-      Distance targetDistance);
+      boolean passing, double distance, Rotation2d targetRobotAngle, Distance targetDistance);
 }

src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java

@@ -10,7 +10,6 @@
 import com.ctre.phoenix6.Utils;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
-import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
 
 /** Add your docs here. */
@@ -81,11 +80,7 @@ public class MappedLaunchRequestBuilder implements LaunchRequestBuilder {
   }
 
   public LaunchRequest createLaunchRequest(
-      boolean passing,
-      double distance,
-      AngularVelocity targetRobotAngularVelocity,
-      Rotation2d targetRobotAngle,
-      Distance targetDistance) {
+      boolean passing, double distance, Rotation2d targetRobotAngle, Distance targetDistance) {
 
     double hoodAngle, flywheelSpeed;
     if (passing) {
@@ -101,7 +96,6 @@ public LaunchRequest createLaunchRequest(
     return new LaunchRequest(
         Rotations.of(hoodAngle),
         RotationsPerSecond.of(flywheelSpeed),
-        targetRobotAngularVelocity,
         targetRobotAngle,
         targetDistance,
         Utils.getCurrentTimeSeconds());

src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java

@@ -17,11 +17,7 @@
 public class ParabolicLaunchRequestBuilder implements LaunchRequestBuilder {
 
   public LaunchRequest createLaunchRequest(
-      boolean passing,
-      double distance,
-      AngularVelocity targetRobotAngularVelocity,
-      Rotation2d targetRobotAngle,
-      Distance targetDistance) {
+      boolean passing, double distance, Rotation2d targetRobotAngle, Distance targetDistance) {
     double y1 = ShooterConstants.SHOOTER_HEIGHT.in(Meters);
     double x2 = distance;
     double y2 = passing ? 0 : ShooterConstants.HUB_HEIGHT.in(Meters);
@@ -77,7 +73,6 @@ public LaunchRequest createLaunchRequest(
     return new LaunchRequest(
         motorAngle,
         angularVelocity,
-        targetRobotAngularVelocity,
         targetRobotAngle,
         targetDistance,
         Utils.getCurrentTimeSeconds());