Ls improved shooting momentum adjust

src/main/java/frc/robot/Robot.java

@@ -142,7 +142,7 @@ public void robotPeriodic() {
                 SmartDashboard.putBoolean("Hub Active?", hubActive());
                 if (Constants.currentMode == Constants.Mode.SIM) {
                     Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose().get());
-                    Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getSimulationPose().get());
+                    Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getSimulationRawOdomPose());
                 }
             }
         }

src/main/java/frc/robot/constants/GameConstants.java

@@ -1,8 +1,6 @@
 package frc.robot.constants;
 
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotBase;
 import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
@@ -36,7 +34,7 @@ public enum Mode {
     public static final boolean ENABLE_LOGGING = true;
 
     //Debugs
-    public static final boolean DEBUG = false;
+    public static final boolean DEBUG = true;
     public static final boolean ARM_DEBUG = true;
 
     //Joystick
@@ -141,13 +139,11 @@ public enum Mode {
     public static final double GRAVITY = 9.81;
     public static final double HUB_HEIGHT = 1.83;
     public static final double SHOOTER_HEIGHT = 0.5;
-    public static final double BLUE_HUB_X_POSITION = 4.6256;
-    public static final double BLUE_HUB_Y_POSITION = 4.0345;
-    public static final double RED_HUB_X_POSITION = 11.9154;
-    public static final double RED_HUB_Y_POSITION = 4.0345;
-    public static final double X_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .05;
-    public static final double Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET = .12;
-
+    public static final Pose2d BLUE_HUB_POS = new Pose2d(4.6256, 4.0345, new Rotation2d());
+    public static final Pose2d RED_HUB_POS = new Pose2d(11.9154, 4.0345, new Rotation2d());
+    public static final Transform2d TURRET_OFFSET = new Transform2d(0.05, 0.12, new Rotation2d()); //TODO: needs value from hardware
+    public static final boolean ACCOUNT_FOR_ANGULAR_MOMENTUM = true;
+    public static final double DISTANCE_BETWEEN_ROBOT_AND_TURRET = TURRET_OFFSET.getTranslation().getNorm();
     // Shift timings
     public static final int SHIFT_1_START = 130;
     public static final int SHIFT_2_START = 105;

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

@@ -1,6 +1,10 @@
 package frc.robot.subsystems;
 
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.Robot;
@@ -27,12 +31,9 @@ public class ControllerSubsystem extends SubsystemBase {
     private static final double STOP_DELAY_SECONDS = 0.5;
 
     // Placeholder target poses until real field target values are finalized
-    private static final Pose2d BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION,
-            Constants.BLUE_HUB_Y_POSITION, Rotation2d.kZero);
-    private static final Pose2d RED_HUB_TARGET_POSE = new Pose2d(Constants.RED_HUB_X_POSITION,
-            Constants.RED_HUB_Y_POSITION, Rotation2d.kZero);
+    private static final Pose2d BLUE_HUB_TARGET_POSE = Constants.BLUE_HUB_POS;
+    private static final Pose2d RED_HUB_TARGET_POSE = Constants.RED_HUB_POS;
     private static final Pose2d SHUTTLE_TARGET_POSE = new Pose2d(1.0, 7.0, Rotation2d.kZero);
-
     private static final String MANUAL_POSE_X_KEY = "controller/ManualPoseX";
     private static final String MANUAL_POSE_Y_KEY = "controller/ManualPoseY";
     private static final String MANUAL_POSE_R_KEY = "controller/ManualPoseRotation";
@@ -84,9 +85,10 @@ public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContai
     @Override
     public void periodic() {
         Pose2d robotPose = getRobotPose();
+        ChassisSpeeds robotSpeeds = getRobotSpeeds();
         ShootState currentState = getCurrentShootState();
         updateStopDelayState(currentState);
-        updateTargets(currentState, robotPose);
+        updateTargets(currentState, robotPose, robotSpeeds);
         if (Constants.DEBUG) {
             SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString());
             SmartDashboard.putNumber(DISTANCE_METERS_KEY, activeTargets.distanceMeters);
@@ -119,6 +121,9 @@ private Pose2d getRobotPose() {
         }
         return drivebase.getPose();
     }
+    private ChassisSpeeds getRobotSpeeds() {
+        return drivebase.getFieldVelocity();
+    }
 
     private boolean shouldUseManualPose() {
         // This can be confusing in case you're on the robot and have disabled the
@@ -141,7 +146,7 @@ private void updateStopDelayState(ShootState currentState) {
         }
     }
 
-    private void updateTargets(ShootState state, Pose2d robotPose) {
+    private void updateTargets(ShootState state, Pose2d robotPose, ChassisSpeeds robotSpeeds) {
         switch (state) {
             case STOPPED -> updateStoppedTargets();
             case FIXED -> useShotTargets(FIXED_TARGETS);
@@ -151,9 +156,9 @@ private void updateTargets(ShootState state, Pose2d robotPose) {
                     // No color, do nothing...
                     useShotTargets(FIXED_TARGETS);
                 } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)) {
-                    useShotTargets(calculateTargetsFromPose(state, BLUE_HUB_PROFILE, robotPosePredictionCalculation(BLUE_HUB_PROFILE.targetPose,robotPose)));
+                    useShotTargets(calculateTargetsFromPose(state, BLUE_HUB_PROFILE, robotPose, robotSpeeds));
                 } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) {
-                    useShotTargets(calculateTargetsFromPose(state, RED_HUB_PROFILE, robotPosePredictionCalculation(RED_HUB_PROFILE.targetPose,robotPose)));
+                    useShotTargets(calculateTargetsFromPose(state, RED_HUB_PROFILE, robotPose, robotSpeeds));
                 } else {
                     // Unknown color, do nothing...
                     useShotTargets(FIXED_TARGETS);
@@ -164,9 +169,9 @@ private void updateTargets(ShootState state, Pose2d robotPose) {
                 if (Robot.allianceColor().isEmpty()) {
                     useShotTargets(FIXED_TARGETS);
                 } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)) {
-                    useShotTargets(calculateTargetsFromPose(state,BLUE_SHUTTLE_PROFILE, robotPose));
+                    useShotTargets(calculateTargetsFromPose(state,BLUE_SHUTTLE_PROFILE, robotPose, robotSpeeds));
                 } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) {
-                    useShotTargets(calculateTargetsFromPose(state, RED_SHUTTLE_PROFILE, robotPose));
+                    useShotTargets(calculateTargetsFromPose(state, RED_SHUTTLE_PROFILE, robotPose, robotSpeeds));
                 } else {
                     useShotTargets(FIXED_TARGETS);
                 }
@@ -200,11 +205,17 @@ private void useShotTargets(ShotTargets shotTargets) {
                 driverEnabled);
     }
 
-    private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile profile, Pose2d robotPose) {
-        double computedDistanceMeters = calculateDistanceMeters(state, robotPose, profile.targetPose);
-        double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile);
-        double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile);
-        double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile);
+    private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile profile, Pose2d robotPose, ChassisSpeeds robotSpeeds) {
+        Twist2d momentumAdjustment = getMomentumAdjustment(robotPose, Constants.ACCOUNT_FOR_ANGULAR_MOMENTUM, robotSpeeds,
+                equalizeFlightTime(calculateDistanceMeters(robotPose, profile.targetPose), robotPose, profile.targetPose, robotSpeeds));
+        PoseControlProfile adjustedProfile = new PoseControlProfile(profile.targetPose, profile.defaultAnglerAngleDegrees, profile.defaultShooterVelocityRpm, profile.defaultTurretAngleDegrees);
+        adjustedProfile.targetPose = profile.targetPose.exp(momentumAdjustment);
+        Logger.recordOutput("adjustedAimPoint", adjustedProfile.targetPose);
+        double computedDistanceMeters = calculateDistanceMeters(state, robotPose, adjustedProfile.targetPose);
+        boolean shootHub = profile == BLUE_HUB_PROFILE || profile == RED_HUB_PROFILE;
+        double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, adjustedProfile, shootHub);
+        double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, adjustedProfile, shootHub);
+        double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, adjustedProfile);
         return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true,
                 true);
     }
@@ -225,6 +236,9 @@ private double calculateDistanceMeters(ShootState state,Pose2d robotPose, Pose2d
         }
     }
 
+    private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) {
+        return Math.max(Constants.MIN_HUB_DISTANCE, Math.min(Constants.MAX_HUB_DISTANCE, robotPose.getTranslation().getDistance(targetPose.getTranslation())));
+    }
     private Pose2d robotPosePredictionCalculation(Pose2d targetPose, Pose2d robotPose) {
         double flightTime = calculateFlightTime(calculateDistanceMeters(ShootState.SHOOTING_HUB,robotPose,targetPose));
         Pose2d robotPoseTransform = new Pose2d(robotPose.getTranslation(), new Rotation2d());
@@ -239,35 +253,73 @@ private Pose2d robotPosePredictionCalculation(Pose2d targetPose, Pose2d robotPos
         }
         return predictedPose;
     }
-    //Flight time derived from testing videos
-    private double calculateFlightTime(double computedDistanceMeters) {
-        return 0.208*computedDistanceMeters + 0.647;
-    }
 
-    private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) {
-        if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {
-            double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET;
-            return 0.169 * distance * distance
-                    - 1.73 * distance
-                    + 20.4;
+    private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile, boolean shootHub) {
+        computedDistanceMeters -= Constants.COMPUTATED_DISTANCE_OFFSET;
+        if (shootHub) {
+            return
+                    1.82 * computedDistanceMeters * computedDistanceMeters
+                            - 5.68 * computedDistanceMeters
+                            + 20.4;
         }
         return profile.defaultAnglerAngleDegrees;
     }
 
-    private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) {
-        if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) {
-            double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET;
-            return (8.46 * distance * distance
-                    - 237 * distance
-                    - 1380);
+
+
+    // Twist2d is a change in pose (position and rotation) over time.
+    // the returned value is a field-relative displacement vector (change in position and rotation) over the ball flight time.
+    // this means how much we need to adjust our shot.
+    public Twist2d getMomentumAdjustment(Pose2d robotPose, boolean useAngularMomentumAdjustment, ChassisSpeeds robotSpeeds, double timeOfFlight) {
+        // calculate the change per second of the turret's position relative to the center due to robot rotation. This
+        // is basically angular speed. Result is in robot coordinate system.
+        ChassisSpeeds adjustSpeeds;
+        if (useAngularMomentumAdjustment) {
+            Translation2d angularVelocityAdjustment = Constants.TURRET_OFFSET.times(robotSpeeds.omegaRadiansPerSecond).getTranslation();
+
+
+            // take the robot's current speed (relative to field)
+            // convert the turret's angular velocity from robot-relative to field-relative
+            // since the rotation velocity is perpendicular to the robot x-axis, so we need to rotate it by 90 degrees
+            // which is why we use -Y,X instead of X,Y for the conversion.
+            // add the two to get the effective speed of the turret/projectile.
+            adjustSpeeds = (new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, 0)).plus
+                    (ChassisSpeeds.fromRobotRelativeSpeeds(-angularVelocityAdjustment.getY(), angularVelocityAdjustment.getX(), 0.0, robotPose.getRotation()));
+        } else {
+            adjustSpeeds = (new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, 0));
+        }
+        // convert the effective speed to a twist (displacement over time)
+        // positive timeOfFlight tells us how much we move
+        // negative timeOfFlight tells us how much we need to move to compensate for our movement
+        return adjustSpeeds.toTwist2d(-timeOfFlight);
+    }
+    // Linear regression through 3 static shot distance vs time points.
+    private double calculateFlightTime(double computedDistanceMeters) {
+        return 0.208*computedDistanceMeters + 0.647;
+    }
+
+    // Since t(x)=mx+b (previous function), we can solve for x
+    // t = m*(d - v_robot * t)+b
+    // t + v_robot * t = m * d + b
+    // t = (m * d + b) / (v_robot + 1)
+    private double equalizeFlightTime(double initialDistanceMeters, Pose2d robotPose, Pose2d target, ChassisSpeeds robotSpeeds) {
+        return (0.208*initialDistanceMeters+0.647)/(0.208*ChassisSpeeds.fromFieldRelativeSpeeds(robotSpeeds, target.getTranslation().minus(robotPose.getTranslation()).getAngle()).vxMetersPerSecond+1);
+    }
+    private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile, boolean shootHub) {
+        computedDistanceMeters -= Constants.COMPUTATED_DISTANCE_OFFSET;
+        if (shootHub) {
+            return
+                    91 * computedDistanceMeters * computedDistanceMeters
+                            - 776 * computedDistanceMeters
+                            - 1_380;
         }
         return profile.defaultShooterVelocityRpm;
     }
 
+
     private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) {
         return Math.floor(
-                Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(),
-                        robotPose.getRotation().getRadians(),
+                Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose, profile.targetPose,
                         Robot.allianceColor().get() == DriverStation.Alliance.Blue)));
     }
 
@@ -332,7 +384,7 @@ private ShotTargets(
     // Class to save all the target poses and each subsystem position at that point
     // so we can calculate true values later on
     private static final class PoseControlProfile {
-        private final Pose2d targetPose;
+        private Pose2d targetPose;
         private final double defaultAnglerAngleDegrees;
         private final double defaultShooterVelocityRpm;
         private final double defaultTurretAngleDegrees;

src/main/java/frc/robot/utils/math/TurretCalculations.java

@@ -1,5 +1,9 @@
 package frc.robot.utils.math;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 
 import org.littletonrobotics.junction.Logger;
 
@@ -32,31 +36,11 @@ public class TurretCalculations {
     // all positions are in meters
     // robotRotation is in radians
     // Returns - turret angle in radians
-    public static double calculateTurretAngle(double robotPosX, double robotPosY, double robotRotation, boolean isBlueAlliance) {
-
-        // Get turret offset from robot center
-        double turretOffsetX = GameConstants.X_DISTANCE_BETWEEN_ROBOT_AND_TURRET;
-        double turretOffsetY = GameConstants.Y_DISTANCE_BETWEEN_ROBOT_AND_TURRET;
-
-        // Rotate the offset by robot rotation (offset rotates with robot)
-        double rotatedOffsetX = turretOffsetX * Math.cos(robotRotation) - turretOffsetY * Math.sin(robotRotation);
-        double rotatedOffsetY = turretOffsetX * Math.sin(robotRotation) + turretOffsetY * Math.cos(robotRotation);
-
-        // Calculate actual turret position on field
-        double turretPosX = robotPosX + rotatedOffsetX;
-        double turretPosY = robotPosY + rotatedOffsetY;
+    public static double calculateTurretAngle(Pose2d robotPos,Pose2d adjTargetPose, boolean isBlueAlliance) {
 
-        double hubPosX;
-        double hubPosY;
+        Pose2d turretPos = robotPos.transformBy(GameConstants.TURRET_OFFSET);
 
-        if (isBlueAlliance) {
-            // hub position determined by which alliance robot is on
-            hubPosX = GameConstants.BLUE_HUB_X_POSITION;
-            hubPosY = GameConstants.BLUE_HUB_Y_POSITION;
-        } else {
-            hubPosX = GameConstants.RED_HUB_X_POSITION;
-            hubPosY = GameConstants.RED_HUB_Y_POSITION;
-        }
+        Pose2d hubPos = adjTargetPose;
 
         /*
          * This finds the unadjusted pan angle (assuming there is no robot rotation) using
@@ -66,15 +50,16 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do
          * of the input numbers.
          * 
          */
-        double panAngleUnadjusted = Math.atan2(hubPosY - turretPosY, hubPosX - turretPosX);
+        Translation2d hubRelativeToRobot = hubPos.relativeTo(turretPos).getTranslation();
+        double panAngleUnadjusted = hubRelativeToRobot.getAngle().getRadians();
 
         /*
          * Adjusts the pan angle to account for the robot's current rotation. We subtract the
          * angle of the robot's rotation from the unadjusted angle of the turret to find the 
          * pan angle, which is the proper angle of the turret adjusted for the robot's rotation
          * and the fact that the turret 0 angle in in the back of the robot.
          */
-        double panAngle = panAngleUnadjusted - (robotRotation + Math.PI);
+        double panAngle = panAngleUnadjusted - (robotPos.getRotation().getRadians() + Math.PI);
 
         // normalize angle between -PI and PI
         double normalizedPanAngle = panAngle - 2 * Math.PI * Math.floor((panAngle + Math.PI) / (2 * Math.PI));