Autonomous

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

@@ -6,6 +6,7 @@
 package frc.trigon.robot;
 
 import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -40,11 +41,17 @@
 import lib.utilities.flippable.Flippable;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
+import java.util.List;
+
 public class RobotContainer {
-    public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator();
+    public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator(
+            CameraConstants.INTAKE_SIDE_REEF_TAG_CAMERA,
+            CameraConstants.LEFT_REEF_TAG_CAMERA,
+            CameraConstants.RIGHT_REEF_TAG_CAMERA
+    );
     public static final ObjectPoseEstimator CORAL_POSE_ESTIMATOR = new ObjectPoseEstimator(
             CameraConstants.OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS,
-            ObjectPoseEstimator.DistanceCalculationMethod.ROTATION_AND_TRANSLATION,
+            ObjectPoseEstimator.DistanceCalculationMethod.TRANSLATION,
             SimulatedGamePieceConstants.GamePieceType.CORAL,
             CameraConstants.OBJECT_DETECTION_CAMERA
     );
@@ -122,7 +129,44 @@ private void configureSysIDBindings(MotorSubsystem subsystem) {
         subsystem.setDefaultCommand(Commands.idle(subsystem));
     }
 
+    @SuppressWarnings("All")
     private void buildAutoChooser() {
-        autoChooser = new LoggedDashboardChooser<>("AutoChooser", AutoBuilder.buildAutoChooser());
+        autoChooser = new LoggedDashboardChooser<>("AutoChooser");
+
+        final List<String> autoNames = AutoBuilder.getAllAutoNames();
+        boolean hasDefault = false;
+
+        for (String autoName : autoNames) {
+            final PathPlannerAuto autoNonMirrored = new PathPlannerAuto(autoName);
+            final PathPlannerAuto autoMirrored = new PathPlannerAuto(autoName, true);
+
+            if (!AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty() && AutonomousConstants.DEFAULT_AUTO_NAME.equals(autoName)) {
+                hasDefault = true;
+                autoChooser.addDefaultOption(autoNonMirrored.getName(), autoNonMirrored);
+                autoChooser.addOption(autoMirrored.getName() + "Mirrored", autoMirrored);
+            } else if (!AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty() && AutonomousConstants.DEFAULT_AUTO_NAME.equals(autoName + "Mirrored")) {
+                hasDefault = true;
+                autoChooser.addDefaultOption(autoMirrored.getName() + "Mirrored", autoMirrored);
+                autoChooser.addOption(autoNonMirrored.getName(), autoNonMirrored);
+            } else {
+                autoChooser.addOption(autoNonMirrored.getName(), autoNonMirrored);
+                autoChooser.addOption(autoMirrored.getName() + "Mirrored", autoMirrored);
+            }
+        }
+
+        if (!hasDefault)
+            autoChooser.addDefaultOption("None", Commands.none());
+        else
+            autoChooser.addOption("None", Commands.none());
+
+        addCommandsToChooser(
+                AutonomousCommands.getFloorAutonomousCommand(true),
+                AutonomousCommands.getFloorAutonomousCommand(false)
+        );
+    }
+
+    private void addCommandsToChooser(Command... commands) {
+        for (Command command : commands)
+            autoChooser.addOption(command.getName(), command);
     }
 }

src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java

@@ -20,7 +20,7 @@
  * A command class to assist in the intaking of a game piece.
  */
 public class IntakeAssistCommand extends ParallelCommandGroup {
-    private static final ProfiledPIDController
+    static final ProfiledPIDController
             X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
             new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
             new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)),
@@ -42,7 +42,7 @@ public IntakeAssistCommand(AssistMode assistMode) {
                 getTrackGamePieceCommand(),
                 GeneralCommands.getContinuousConditionalCommand(
                         GeneralCommands.getFieldRelativeDriveCommand(),
-                        getAssistIntakeCommand(assistMode, () -> distanceFromTrackedGamePiece),
+                        getAssistIntakeCommand(assistMode, () -> distanceFromTrackedGamePiece, OperatorConstants.INTAKE_ASSIST_SCALAR),
                         () -> RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null || distanceFromTrackedGamePiece == null
                 )
         );
@@ -57,50 +57,62 @@ public IntakeAssistCommand(AssistMode assistMode) {
      * @param distanceFromTrackedGamePiece the position of the game piece relative to the robot
      * @return the command
      */
-    public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier<Translation2d> distanceFromTrackedGamePiece) {
+    public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier<Translation2d> distanceFromTrackedGamePiece, double intakeAssistScalar) {
         return new SequentialCommandGroup(
                 new InstantCommand(() -> resetPIDControllers(distanceFromTrackedGamePiece.get())),
                 SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
-                        () -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get()).getX(),
-                        () -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get()).getY(),
-                        () -> calculateThetaPower(assistMode, distanceFromTrackedGamePiece.get())
+                        () -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get(), intakeAssistScalar).getX(),
+                        () -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get(), intakeAssistScalar).getY(),
+                        () -> calculateThetaPower(assistMode, distanceFromTrackedGamePiece.get(), intakeAssistScalar)
                 )
         );
     }
 
     private Command getTrackGamePieceCommand() {
         return new RunCommand(() -> {
             if (RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null)
-                distanceFromTrackedGamePiece = calculateDistanceFromTrackedCGamePiece();
+                distanceFromTrackedGamePiece = calculateDistanceFromTrackedGamePiece();
         });
     }
 
-    private static Translation2d calculateDistanceFromTrackedCGamePiece() {
+    public static Translation2d calculateDistanceFromTrackedGamePiece() {
         final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
         final Translation2d trackedObjectPositionOnField = RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot();
         if (trackedObjectPositionOnField == null)
             return null;
 
         final Translation2d difference = robotPose.getTranslation().minus(trackedObjectPositionOnField);
-        final Translation2d robotToTrackedGamepieceDistance = difference.rotateBy(robotPose.getRotation().unaryMinus());
-        Logger.recordOutput("IntakeAssist/TrackedGamePieceDistance", robotToTrackedGamepieceDistance);
-        return robotToTrackedGamepieceDistance;
+        final Translation2d robotToTrackedGamePieceDistance = difference.rotateBy(robotPose.getRotation().unaryMinus());
+        Logger.recordOutput("IntakeAssist/TrackedGamePieceDistance", robotToTrackedGamePieceDistance);
+        return robotToTrackedGamePieceDistance;
     }
 
-    private static Translation2d calculateTranslationPower(AssistMode assistMode, Translation2d distanceFromTrackedGamepiece) {
+    private static Translation2d calculateTranslationPower(AssistMode assistMode, Translation2d distanceFromTrackedGamePiece, double intakeAssistScalar) {
+        if (distanceFromTrackedGamePiece == null)
+            return new Translation2d(0, 0);
         final Translation2d joystickPower = new Translation2d(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX());
         final Translation2d selfRelativeJoystickPower = joystickPower.rotateBy(RobotContainer.SWERVE.getDriveRelativeAngle().unaryMinus());
 
-        final double xPIDOutput = clampToOutputRange(X_PID_CONTROLLER.calculate(distanceFromTrackedGamepiece.getX()));
-        final double yPIDOutput = clampToOutputRange(Y_PID_CONTROLLER.calculate(distanceFromTrackedGamepiece.getY()));
+        final double xPIDOutput = clampToOutputRange(X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.getX()));
+        final double yPIDOutput = clampToOutputRange(Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.getY()));
 
         if (assistMode.equals(AssistMode.ALTERNATE_ASSIST))
             return calculateAlternateAssistTranslationPower(selfRelativeJoystickPower, xPIDOutput, yPIDOutput);
-        return calculateNormalAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput);
+        return calculateNormalAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput, intakeAssistScalar);
     }
 
-    private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedGamepiece) {
-        return calculateThetaAssistPower(assistMode, distanceFromTrackedGamepiece.getAngle().plus(Rotation2d.k180deg).unaryMinus());
+    private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedGamePiece, double intakeAssistScalar) {
+        if (distanceFromTrackedGamePiece == null || !assistMode.shouldAssistTheta)
+            return 0;
+        Rotation2d distanceAngle = distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg).unaryMinus();
+        Rotation2d robotAngle = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation();
+        Rotation2d diff = robotAngle.minus(distanceAngle);
+        Logger.recordOutput("IntakeAssist/difference", diff);
+        Logger.recordOutput("IntakeAssist/robotAngle", robotAngle);
+        Logger.recordOutput("IntakeAssist/distanceAngle", distanceAngle);
+        var pow = calculateThetaAssistPower(assistMode, distanceAngle, intakeAssistScalar);
+        Logger.recordOutput("IntakeAssist/pow", pow);
+        return pow;
     }
 
     private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) {
@@ -115,25 +127,25 @@ private static Translation2d calculateAlternateAssistTranslationPower(Translatio
         return new Translation2d(xPower, yPower);
     }
 
-    private static Translation2d calculateNormalAssistTranslationPower(AssistMode assistMode, Translation2d joystickValue, double xPIDOutput, double yPIDOutput) {
+    private static Translation2d calculateNormalAssistTranslationPower(AssistMode assistMode, Translation2d joystickValue, double xPIDOutput, double yPIDOutput, double intakeAssistScalar) {
         final double
                 xJoystickPower = joystickValue.getX(),
                 yJoystickPower = joystickValue.getY();
         final double
-                xPower = assistMode.shouldAssistX ? calculateNormalAssistPower(xPIDOutput, xJoystickPower) : xJoystickPower,
-                yPower = assistMode.shouldAssistY ? calculateNormalAssistPower(yPIDOutput, yJoystickPower) : yJoystickPower;
+                xPower = assistMode.shouldAssistX ? calculateNormalAssistPower(xPIDOutput, xJoystickPower, intakeAssistScalar) : xJoystickPower,
+                yPower = assistMode.shouldAssistY ? calculateNormalAssistPower(yPIDOutput, yJoystickPower, intakeAssistScalar) : yJoystickPower;
 
         return new Translation2d(xPower, yPower);
     }
 
-    private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2d thetaOffset) {
+    private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2d thetaOffset, double intakeAssistScalar) {
         final double
                 pidOutput = clampToOutputRange(THETA_PID_CONTROLLER.calculate(thetaOffset.getRadians())),
                 joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX();
 
         if (assistMode.equals(AssistMode.ALTERNATE_ASSIST))
             return calculateAlternateAssistPower(pidOutput, joystickValue, joystickValue);
-        return calculateNormalAssistPower(pidOutput, joystickValue);
+        return calculateNormalAssistPower(pidOutput, joystickValue, intakeAssistScalar);
     }
 
     private static double clampToOutputRange(double value) {
@@ -144,15 +156,14 @@ private static double calculateAlternateAssistPower(double pidOutput, double pid
         return pidOutput * (1 - Math.abs(pidScalar)) + joystickPower;
     }
 
-    private static double calculateNormalAssistPower(double pidOutput, double joystickPower) {
-        final double intakeAssistScalar = OperatorConstants.INTAKE_ASSIST_SCALAR;
-        return (pidOutput * intakeAssistScalar) + (joystickPower * (1 - intakeAssistScalar));
+    private static double calculateNormalAssistPower(double pidOutput, double joystickPower, double scalar) {
+        return (pidOutput * scalar) + (joystickPower * (1 - scalar));
     }
 
     private static void resetPIDControllers(Translation2d distanceFromTrackedGamePiece) {
         X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond);
         Y_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getY(), RobotContainer.SWERVE.getSelfRelativeVelocity().vyMetersPerSecond);
-        THETA_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getAngle().getRadians(), RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond);
+        THETA_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg).unaryMinus().getRadians(), RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond);
     }
 
     /**
@@ -164,15 +175,15 @@ public enum AssistMode {
          */
         ALTERNATE_ASSIST(true, true, true),
         /**
-         * Applies pid values to autonomously drive to the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs
+         * Applies pid values to autonomously drive to the game piece, scaled by the intake assist scalar in addition to the driver's inputs
          */
         FULL_ASSIST(true, true, true),
         /**
-         * Applies pid values to align to the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs
+         * Applies pid values to align to the game piece, scaled by the intake assist scalar in addition to the driver's inputs
          */
         ALIGN_ASSIST(false, true, true),
         /**
-         * Applies pid values to face the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs
+         * Applies pid values to face the game piece, scaled by the intake assist scalar in addition to the driver's inputs
          */
         ASSIST_ROTATION(false, false, true);