added pose prediction + added misc changes from our testing branch

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

@@ -307,7 +307,7 @@ private void configureBindings() {
                 controller.y().onTrue(new ClimberUp(climberSubsystem));
                 controller.x().onTrue(new ClimberDown(climberSubsystem));
                 controller.povUp().onTrue(new SetShootingState(shootState, ShootState.FIXED));
-                controller.povRight().onTrue(new SetShootingState(shootState, ShootState.FIXED_2));
+                controller.povRight().onTrue(new SetShootingState(shootState, ShootState.STOPPED));
                 controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB));
                 controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING));
                 driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED)));
@@ -346,7 +346,7 @@ private void configureBindings() {
                         SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
                                         () -> driveJoystick.getY() * -1,
                                         () -> driveJoystick.getX() * -1)
-                                        .withControllerRotationAxis(steerJoystick::getX)
+                                        .withControllerRotationAxis(() -> {return steerJoystick.getX() * -1;})
                                         .deadband(Constants.DEADBAND)
                                         .scaleTranslation(0.8)
                                         .allianceRelativeControl(true);

src/main/java/frc/robot/autochooser/AutoChooser.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Robot;
+import frc.robot.commands.auto.DoNothing;
 import frc.robot.commands.auto.shoot.BlueDepotShoot;
 import frc.robot.commands.auto.shoot.BlueMidShoot;
 import frc.robot.commands.auto.shoot.BlueOutpostShoot;
@@ -30,7 +31,6 @@
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
-import frc.robot.utils.logging.commands.DoNothingCommand;
 
 public class AutoChooser {
 
@@ -106,11 +106,11 @@ private void populateChoosers() {
     private void populateCommandMap() {
         //if AutoEvent is not dependent on alliance color don't put a color
         commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.DEPOT_SIDE),
-            new DoNothingCommand());
+            new DoNothing(turret, angler));
         commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.MID),
-            new DoNothingCommand());
+            new DoNothing(turret, angler));
         commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.OUTPOST_SIDE), 
-            new DoNothingCommand());
+            new DoNothing(turret, angler));
 
         commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.DEPOT_SIDE, Alliance.Blue),
             new BlueDepotShoot(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller)); //shoot depot blue

src/main/java/frc/robot/commands/auto/DoNothing.java

@@ -1,13 +1,19 @@
 package frc.robot.commands.auto;
 
-import frc.robot.utils.logging.commands.LoggableCommand;
+import frc.robot.commands.angler.RunAnglerToReverseLimit;
+import frc.robot.commands.turret.RunTurretToRevLimit;
+import frc.robot.subsystems.AnglerSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+import frc.robot.utils.logging.commands.LoggableParallelCommandGroup;
 
-public class DoNothing extends LoggableCommand{
-    public DoNothing() {
-    }
+public class DoNothing extends LoggableParallelCommandGroup{
+    public DoNothing(TurretSubsystem turret, AnglerSubsystem angler) {
+            super(
+                new LoggableParallelCommandGroup(
+                    new RunTurretToRevLimit(turret),
+                    new RunAnglerToReverseLimit(angler)
+                )
 
-    @Override
-    public boolean isFinished() {
-        return true;
+            );
     }
 }

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

@@ -170,4 +170,7 @@ public enum Mode {
     public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later
     public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later
     public static final double MAX_VISION_DISTANCE_SIMULATION = 6;
+
+    public static final double MAX_HUB_DISTANCE = 5.18;
+    public static final double MIN_HUB_DISTANCE = 1.42;
 }

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

@@ -1,15 +1,20 @@
 package frc.robot.subsystems;
 
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.Robot;
 import frc.robot.utils.math.TurretCalculations;
+
+import java.util.ArrayList;
+
 import org.dyn4j.UnitConversion;
 import org.littletonrobotics.junction.Logger;
 
 import frc.robot.RobotContainer;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -22,8 +27,10 @@ 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 = 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 SHUTTLE_TARGET_POSE = new Pose2d(1.0, 7.0, Rotation2d.kZero);
 
     private static final String MANUAL_POSE_X_KEY = "controller/ManualPoseX";
@@ -39,20 +46,18 @@ public class ControllerSubsystem extends SubsystemBase {
     private static final String TARGET_HOPPER_SPEED_KEY = "controller/TargetHopperSpeed";
 
     // Placeholder fixed-state settings.
-    private static final ShotTargets STOPPED_TARGETS =
-            new ShotTargets(Constants.ANGLER_ANGLE_LOW, 0.0, 0.0, 0.0, false, false);
-    private static final ShotTargets FIXED_TARGETS =
-            new ShotTargets(10.0, 120.0, 5.0, 0.0, true, true);
-    private static final ShotTargets FIXED_2_TARGETS =
-            new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true);
+    private static final ShotTargets STOPPED_TARGETS = new ShotTargets(Constants.ANGLER_ANGLE_LOW, 0.0, 0.0, 0.0, false,
+            false);
+    private static final ShotTargets FIXED_TARGETS = new ShotTargets(10.0, 120.0, 5.0, 0.0, true, true);
+    private static final ShotTargets FIXED_2_TARGETS = new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true);
 
     // Placeholder pose-driven profiles.
-    private static final PoseControlProfile BLUE_HUB_PROFILE =
-            new PoseControlProfile(BLUE_HUB_TARGET_POSE, 32.0, 230.0, 14.0);
-    private static final PoseControlProfile RED_HUB_PROFILE =
-            new PoseControlProfile(RED_HUB_TARGET_POSE, 32.0, 230.0, 14.0);
-    private static final PoseControlProfile SHUTTLE_PROFILE =
-            new PoseControlProfile(SHUTTLE_TARGET_POSE, 16.0, 90.0, -14.0);
+    private static final PoseControlProfile BLUE_HUB_PROFILE = new PoseControlProfile(BLUE_HUB_TARGET_POSE, 32.0, 230.0,
+            14.0);
+    private static final PoseControlProfile RED_HUB_PROFILE = new PoseControlProfile(RED_HUB_TARGET_POSE, 32.0, 230.0,
+            14.0);
+    private static final PoseControlProfile SHUTTLE_PROFILE = new PoseControlProfile(SHUTTLE_TARGET_POSE, 16.0, 90.0,
+            -14.0);
 
     private final SwerveSubsystem drivebase;
     private final RobotContainer robotContainer;
@@ -62,7 +67,6 @@ public class ControllerSubsystem extends SubsystemBase {
     private ShotTargets activeTargets;
     private boolean driverActivatedShooting = false;
 
-
     public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContainer) {
         this.drivebase = drivebase;
         this.robotContainer = robotContainer;
@@ -78,10 +82,9 @@ public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContai
     public void periodic() {
         Pose2d robotPose = getRobotPose();
         ShootState currentState = getCurrentShootState();
-
         updateStopDelayState(currentState);
         updateTargets(currentState, robotPose);
-        if(Constants.DEBUG){
+        if (Constants.DEBUG) {
             SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString());
             SmartDashboard.putNumber(DISTANCE_METERS_KEY, activeTargets.distanceMeters);
             SmartDashboard.putNumber(TARGET_ANGLER_ANGLE_KEY, activeTargets.anglerAngleDegrees);
@@ -115,7 +118,8 @@ private Pose2d getRobotPose() {
     }
 
     private boolean shouldUseManualPose() {
-        // This can be confusing in case you're on the robot and have disabled the drivetrain
+        // This can be confusing in case you're on the robot and have disabled the
+        // drivetrain
         // Uncomment to control manually
         return false;
     }
@@ -124,7 +128,7 @@ private Pose2d getManualPose() {
         double x = SmartDashboard.getNumber(MANUAL_POSE_X_KEY, 0.0);
         double y = SmartDashboard.getNumber(MANUAL_POSE_Y_KEY, 0.0);
         double r = SmartDashboard.getNumber(MANUAL_POSE_R_KEY, 0.0);
-        Logger.recordOutput("Manual Pose", new Pose2d(new Translation2d(x,y), new Rotation2d(r)));
+        Logger.recordOutput("Manual Pose", new Pose2d(new Translation2d(x, y), new Rotation2d(r)));
         return new Pose2d(x, y, new Rotation2d(r));
     }
 
@@ -144,21 +148,21 @@ 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(BLUE_HUB_PROFILE, robotPose));
+                    useShotTargets(calculateTargetsFromPose(BLUE_HUB_PROFILE, robotPosePredictionCalculation(BLUE_HUB_PROFILE.targetPose,robotPose)));
                 } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) {
-                    useShotTargets(calculateTargetsFromPose(RED_HUB_PROFILE, robotPose));
+                    useShotTargets(calculateTargetsFromPose(RED_HUB_PROFILE, robotPosePredictionCalculation(RED_HUB_PROFILE.targetPose,robotPose)));
                 } else {
                     // Unknown color, do nothing...
                     useShotTargets(FIXED_TARGETS);
                 }
             }
 
-            case SHUTTLING -> useShotTargets(calculateTargetsFromPose(SHUTTLE_PROFILE, robotPose));
+            case SHUTTLING -> useShotTargets(calculateTargetsFromPose(SHUTTLE_PROFILE, robotPosePredictionCalculation(SHUTTLE_PROFILE.targetPose,robotPose)));
         }
     }
 
     private void updateStoppedTargets() {
-        //This makes the shooter wait half a second before stopping
+        // This makes the shooter wait half a second before stopping
         double shooterVelocity = stopDelayTimer.hasElapsed(STOP_DELAY_SECONDS)
                 ? 0.0
                 : activeTargets.shooterVelocityRpm;
@@ -188,40 +192,66 @@ private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d
         double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile);
         double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile);
         double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile);
-        return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, true);
+        return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true,
+                true);
     }
 
     private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) {
-        return robotPose.getTranslation().getDistance(targetPose.getTranslation());
+        double distance = robotPose.getTranslation()
+                .getDistance(targetPose.getTranslation());
+        if (distance > Constants.MAX_HUB_DISTANCE) {
+            return Constants.MAX_HUB_DISTANCE;
+        } else if (distance < Constants.MIN_HUB_DISTANCE) {
+            return Constants.MIN_HUB_DISTANCE;
+        } else {
+            return distance;
+        }
+    }
+
+    private Pose2d robotPosePredictionCalculation(Pose2d targetPose, Pose2d robotPose) {
+        double flightTime = calculateFlightTime(robotPose.getTranslation()
+                .getDistance(targetPose.getTranslation()));
+        Pose2d robotPoseTransform = new Pose2d(robotPose.getTranslation(), new Rotation2d());
+        Pose2d predictedPose = robotPoseTransform
+                        .plus(new Transform2d(
+                        drivebase.getFieldVelocity().vxMetersPerSecond * flightTime,
+                        drivebase.getFieldVelocity().vyMetersPerSecond * flightTime, 
+                        new Rotation2d()));
+        Logger.recordOutput("Predicted pose", predictedPose);
+        return predictedPose;
+    }
+    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;
-			return
-				0.169 * distance * distance
-			  - 1.73 * distance
-	          + 20.4;
-		}
+            double distance = (3.281 * computedDistanceMeters) - 2;
+            return 0.169 * distance * distance
+                    - 1.73 * distance
+                    + 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;
-			return Math.floor(
-				8.46 * distance * distance
-			  - 237 * distance
-			  - 1_380);
-		}
+            double distance = (3.81 * computedDistanceMeters) - 2;
+            return (8.46 * distance * distance
+                    - 237 * distance
+                    - 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(), DriverStation.getAlliance().get()== DriverStation.Alliance.Blue)));
+        return Math.floor(
+                Math.toDegrees(TurretCalculations.calculateTurretAngle(robotPose.getX(), robotPose.getY(),
+                        robotPose.getRotation().getRadians(),
+                        Robot.allianceColor().get() == DriverStation.Alliance.Blue)));
     }
 
-    //Getters for all the subsystems to set posistion.
+    // Getters for all the subsystems to set posistion.
     public double getTargetAnglerAngleDegrees() {
         return activeTargets.anglerAngleDegrees;
     }
@@ -254,8 +284,7 @@ public boolean driverActivatedShootingEnabled() {
         return driverActivatedShooting;
     }
 
-
-    //Class to save all the fixed targets
+    // Class to save all the fixed targets
     private static final class ShotTargets {
         private final double anglerAngleDegrees;
         private final double shooterVelocityRpm;
@@ -280,7 +309,8 @@ private ShotTargets(
         }
     }
 
-    //Class to save all the target poses and each subsystem position at that point so we can calculate true values later on
+    // 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 final double defaultAnglerAngleDegrees;

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

@@ -1,5 +1,11 @@
 package frc.robot.utils.math;
 
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import frc.robot.constants.GameConstants;
 
 public class TurretCalculations {
@@ -72,6 +78,7 @@ public static double calculateTurretAngle(double robotPosX, double robotPosY, do
 
         // normalize angle between -PI and PI
         double normalizedPanAngle = panAngle - 2 * Math.PI * Math.floor((panAngle + Math.PI) / (2 * Math.PI));
+        Logger.recordOutput("TurretPose", new Pose2d(new Translation2d(turretPosX,turretPosY), new Rotation2d(normalizedPanAngle)));
         return normalizedPanAngle;
     }