Autonomous

src/main/deploy/pathplanner/settings.json

@@ -15,7 +15,7 @@
   "robotMOI": 5.018,
   "robotTrackwidth": 0.546,
   "driveWheelRadius": 0.049,
-  "driveGearing": 6.12,
+  "driveGearing": 7.03,
   "maxDriveSpeed": 4.4,
   "driveMotorType": "krakenX60FOC",
   "driveCurrentLimit": 100.0,

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

@@ -5,12 +5,12 @@
 
 package frc.trigon.robot;
 
-import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.trigon.lib.utilities.flippable.Flippable;
 import frc.trigon.robot.commands.CommandConstants;
+import frc.trigon.robot.commands.commandfactories.autonomous.GeneralAutonomousCommands;
 import frc.trigon.robot.commands.commandfactories.FuelIntakeCommands;
 import frc.trigon.robot.commands.commandfactories.GeneralCommands;
 import frc.trigon.robot.commands.commandfactories.ShootingCommands;
@@ -38,7 +38,6 @@
 import frc.trigon.robot.subsystems.swerve.Swerve;
 import frc.trigon.robot.subsystems.turret.Turret;
 import frc.trigon.robot.subsystems.turret.TurretCommands;
-import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 public class RobotContainer {
     public static final RobotPoseEstimator ROBOT_POSE_ESTIMATOR = new RobotPoseEstimator();
@@ -54,25 +53,23 @@ public class RobotContainer {
     public static final Shooter SHOOTER = new Shooter();
     public static final Spindexer SPINDEXER = new Spindexer();
     public static final Turret TURRET = new Turret();
-    private LoggedDashboardChooser<Command> autoChooser;
 
     public RobotContainer() {
         initializeGeneralSystems();
-        buildAutoChooser();
         configureBindings();
     }
 
     /**
      * @return the command to run in autonomous mode
      */
     public Command getAutonomousCommand() {
-        return autoChooser.get();
+        return GeneralAutonomousCommands.getAutonomousCommand();
     }
 
     private void configureBindings() {
         bindDefaultCommands();
         bindControllerCommands();
-//        configureSysIDBindings(TURRET);
+//        configureSysIDBindings(SWERVE);
     }
 
     private void bindDefaultCommands() {
@@ -103,7 +100,7 @@ private void bindControllerCommands() {
         OperatorConstants.SET_FIXED_SHOOTING_POSITION_LEFT_CORNER_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.LEFT_CORNER));
         OperatorConstants.SET_FIXED_SHOOTING_POSITION_CLOSE_TO_TOWER_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.CLOSE_TO_TOWER));
         OperatorConstants.SET_FIXED_SHOOTING_POSITION_CLOSE_TO_OUTPOST_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.CLOSE_TO_OUTPOST));
-        OperatorConstants.SHORT_EJECTION_TRIGGER.whileTrue(ShootingCommands.getShortEjectFuelCommand());
+        OperatorConstants.SHORT_EJECTION_TRIGGER.whileTrue(TurretCommands.getDebuggingCommand());
     }
 
     private void configureSysIDBindings(MotorSubsystem subsystem) {
@@ -125,8 +122,4 @@ private void initializeGeneralSystems() {
         AutonomousConstants.init();
         ShootingLookupTable3D.init();
     }
-
-    private void buildAutoChooser() {
-        autoChooser = new LoggedDashboardChooser<>("AutoChooser", AutoBuilder.buildAutoChooser());
-    }
 }

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

@@ -1,16 +1,17 @@
 package frc.trigon.robot.commands.commandclasses;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.*;
 import frc.trigon.lib.hardware.RobotHardwareStats;
 import frc.trigon.lib.utilities.flippable.Flippable;
 import frc.trigon.robot.RobotContainer;
+import frc.trigon.robot.commands.CommandConstants;
 import frc.trigon.robot.constants.OperatorConstants;
+import frc.trigon.robot.misc.shootingphysics.ShootingCalculations;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
 import frc.trigon.robot.subsystems.swerve.SwerveConstants;
 import org.littletonrobotics.junction.Logger;
@@ -21,26 +22,26 @@
  * A command class to assist in the intaking of a game piece.
  */
 public class IntakeAssistCommand extends ParallelCommandGroup {
-    private final ProfiledPIDController
+    private final PIDController
             xPIDController = RobotHardwareStats.isSimulation() ?
-            new ProfiledPIDController(5, 0, 0, new TrapezoidProfile.Constraints(5, 15)) :
-            new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)),
+            new PIDController(5, 0, 0) :
+            new PIDController(2.4, 0, 0),
             yPIDController = RobotHardwareStats.isSimulation() ?
-                    new ProfiledPIDController(3, 0, 0.01, new TrapezoidProfile.Constraints(5, 15)) :
-                    new ProfiledPIDController(0.3, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 5.5)),
+                    new PIDController(0.3, 0, 0) :
+                    new PIDController(0.26, 0, 0),
             thetaPIDController = RobotHardwareStats.isSimulation() ?
-                    new ProfiledPIDController(0.1, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
-                    new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5));
+                    new PIDController(0.1, 0, 0) :
+                    new PIDController(2.4, 0, 0);
     private Translation2d distanceFromTrackedGamePiece;
 
     /**
-     * Creates a new intake assist command that assists the driver with intaking a game piece based on the powers.
+     * Creates a new intake assist command that assists the driver with intaking a game piece.
      *
-     * @param xAssistPower     the amount of assist to apply to the X-axis (Forward and backwards)
-     * @param yAssistPower     the amount of assist to apply to the Y-axis (Sideways)
-     * @param thetaAssistPower the amount of assist to apply to the rotation
+     * @param shouldAssistX     should apply to the X-axis (Forward and backwards)
+     * @param shouldAssistY     should apply to the Y-axis (Sideways)
+     * @param shouldAssistTheta should apply to the rotation
      */
-    public IntakeAssistCommand(double xAssistPower, double yAssistPower, double thetaAssistPower) {
+    public IntakeAssistCommand(boolean shouldAssistX, boolean shouldAssistY, boolean shouldAssistTheta) {
         addCommands(
                 new RunCommand(this::trackGamePiece),
                 new SequentialCommandGroup(
@@ -49,56 +50,54 @@ public IntakeAssistCommand(double xAssistPower, double yAssistPower, double thet
                         new WaitUntilCommand(this::hasNoTrackedGamePiece)
                 ).repeatedly(),
                 SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
-                        () -> calculateTranslationPower(true, xAssistPower),
-                        () -> calculateTranslationPower(false, yAssistPower),
-                        () -> calculateThetaPower(thetaAssistPower)
+                        () -> calculateTranslationPower(true, shouldAssistX),
+                        () -> calculateTranslationPower(false, shouldAssistY),
+                        () -> calculateThetaPower(shouldAssistTheta)
                 )
         );
     }
 
     private void resetPIDControllers() {
-        xPIDController.reset(distanceFromTrackedGamePiece.getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().getX());
-        yPIDController.reset(distanceFromTrackedGamePiece.getY(), RobotContainer.SWERVE.getSelfRelativeVelocity().getY());
-        thetaPIDController.reset(distanceFromTrackedGamePiece.getAngle().getRadians(), RobotContainer.SWERVE.getRotationalVelocityRadiansPerSecond());
+        xPIDController.reset();
+        yPIDController.reset();
+        thetaPIDController.reset();
 
-        xPIDController.setGoal(0);
-        yPIDController.setGoal(0);
-        thetaPIDController.setGoal(0);
+        xPIDController.setSetpoint(0);
+        yPIDController.setSetpoint(0);
+        thetaPIDController.setSetpoint(0);
     }
 
-    private double calculateTranslationPower(boolean isXAxis, double assistScalar) {
+    private double calculateTranslationPower(boolean isXAxis, boolean shouldAssist) {
         final Translation2d selfRelativeJoystickValue = getSelfRelativeJoystickPosition();
-        final double joystickValue = isXAxis ? selfRelativeJoystickValue.getX() : selfRelativeJoystickValue.getY();
+        final double joystickValue = CommandConstants.calculateDriveStickAxisValue(isXAxis ? selfRelativeJoystickValue.getX() : selfRelativeJoystickValue.getY());
 
-        if (hasNoTrackedGamePiece())
+        if (!shouldAssist || hasNoTrackedGamePiece())
             return joystickValue;
 
         final double
                 pidOutput = clampToSwerveOutputRange(isXAxis ?
                 xPIDController.calculate(distanceFromTrackedGamePiece.getX()) :
                 yPIDController.calculate(distanceFromTrackedGamePiece.getY()));
 
-        return calculateAssistPower(assistScalar, pidOutput, joystickValue);
+        return calculateAssistPower(pidOutput, joystickValue);
     }
 
-    private double calculateThetaPower(double assistScalar) {
-        final double joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX();
+    private double calculateThetaPower(boolean shouldAssist) {
+        final double joystickValue = CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getRightX());
 
-        if (hasNoTrackedGamePiece())
+        if (!shouldAssist || hasNoTrackedGamePiece())
             return joystickValue;
 
-        return calculateThetaAssistPower(assistScalar, joystickValue, distanceFromTrackedGamePiece.getAngle().unaryMinus());
+        return calculateThetaAssistPower(joystickValue, distanceFromTrackedGamePiece.getAngle().unaryMinus());
     }
 
-    private double calculateThetaAssistPower(double assistScalar, double joystickValue, Rotation2d thetaOffset) {
+    private double calculateThetaAssistPower(double joystickValue, Rotation2d thetaOffset) {
         final double pidOutput = clampToSwerveOutputRange(thetaPIDController.calculate(thetaOffset.getRadians()));
-        return calculateAssistPower(assistScalar, pidOutput, joystickValue);
+        return calculateAssistPower(pidOutput, joystickValue);
     }
 
-    private double calculateAssistPower(double assistScalar, double pidOutput, double joystickValue) {
-        if (assistScalar == 0)
-            return joystickValue;
-        return (joystickValue * (1 - assistScalar)) + (pidOutput * assistScalar);
+    private double calculateAssistPower(double pidOutput, double joystickValue) {
+        return joystickValue + pidOutput;
     }
 
     private boolean hasNoTrackedGamePiece() {
@@ -165,28 +164,17 @@ private boolean isGamePieceWithinAssistFOV(Translation2d gamePiecePosition, Pose
     }
 
     private boolean isDrivingTowardsGamePiece(Translation2d gamePieceFieldRelativePosition, Pose2d robotPose) {
-        final Translation2d fieldRelativeDistanceFromGamePiece = gamePieceFieldRelativePosition.minus(robotPose.getTranslation());
-
-        final double velocityTowardsGamePiece = calculateVelocityTowardsGamePiece(fieldRelativeDistanceFromGamePiece);
+        final Translation2d velocityRelativeToGamePiece = ShootingCalculations.calculateVelocityRelativeToPoint(gamePieceFieldRelativePosition, robotPose.getTranslation(), getFieldRelativeJoystickPosition());
+        if (velocityRelativeToGamePiece.getX() < Math.abs(velocityRelativeToGamePiece.getY()))
+            return false;
+        final double velocityTowardsGamePiece = velocityRelativeToGamePiece.getX() * SwerveConstants.MAXIMUM_SPEED_METERS_PER_SECOND;
         return velocityTowardsGamePiece > OperatorConstants.MINIMUM_VELOCITY_TOWARDS_GAME_PIECE_FOR_INTAKE_ASSIST_METERS_PER_SECOND;
     }
 
     private double calculateMaximumAssistAngleDegrees(double gamePieceDistance) {
         return OperatorConstants.INTAKE_ASSIST_MAXIMUM_ASSISTABLE_ANGLE_FORMULA.applyAsDouble(gamePieceDistance);
     }
 
-    private double calculateVelocityTowardsGamePiece(Translation2d fieldRelativeDistanceFromGamePiece) {
-        final Translation2d fieldRelativeJoystickPosition = getFieldRelativeJoystickPosition();
-
-        final Rotation2d joystickAngle = fieldRelativeJoystickPosition.getAngle();
-        final Rotation2d fieldRelativeGamePieceAngleFromRobot = fieldRelativeDistanceFromGamePiece.getAngle();
-        final Rotation2d angularOffset = joystickAngle.minus(fieldRelativeGamePieceAngleFromRobot);
-
-        final double desiredRobotVelocity = fieldRelativeJoystickPosition.getNorm() * SwerveConstants.MAXIMUM_SPEED_METERS_PER_SECOND;
-
-        return angularOffset.getCos() * desiredRobotVelocity;
-    }
-
     private double clampToSwerveOutputRange(double value) {
         return MathUtil.clamp(value, -1, 1);
     }

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

@@ -7,7 +7,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.commands.commandfactories.AutonomousCommands;
+import frc.trigon.robot.commands.commandfactories.autonomous.GeneralAutonomousCommands;
 import org.littletonrobotics.junction.Logger;
 
 import java.util.function.Supplier;
@@ -44,7 +44,7 @@ public boolean runsWhenDisabled() {
 
     private Command getUpdateAutoStartPoseCommand() {
         return new InstantCommand(() -> {
-            this.autoStartPose = AutonomousCommands.getAutoStartPose(autoName.get());
+            this.autoStartPose = GeneralAutonomousCommands.getAutoStartPose(autoName.get());
             Logger.recordOutput("PathPlanner/AutoStartPose", autoStartPose);
         });
     }