Dev

src/main/deploy/pathplanner/paths/Example Path.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 400.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/settings.json

@@ -18,7 +18,7 @@
   "driveGearing": 6.12,
   "maxDriveSpeed": 4.4,
   "driveMotorType": "krakenX60FOC",
-  "driveCurrentLimit": 100.0,
+  "driveCurrentLimit": 120.0,
   "wheelCOF": 1.6,
   "flModuleX": 0.172,
   "flModuleY": 0.273,

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

@@ -13,24 +13,18 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.trigon.robot.commands.CommandConstants;
 import frc.trigon.robot.commands.commandfactories.*;
-import frc.trigon.robot.constants.AutonomousConstants;
-import frc.trigon.robot.constants.CameraConstants;
-import frc.trigon.robot.constants.LEDConstants;
-import frc.trigon.robot.constants.OperatorConstants;
+import frc.trigon.robot.constants.*;
 import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator;
 import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
 import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler;
 import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
 import frc.trigon.robot.subsystems.MotorSubsystem;
 import frc.trigon.robot.subsystems.armelevator.ArmElevator;
 import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands;
-import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants;
 import frc.trigon.robot.subsystems.climber.Climber;
 import frc.trigon.robot.subsystems.climber.ClimberCommands;
-import frc.trigon.robot.subsystems.climber.ClimberConstants;
 import frc.trigon.robot.subsystems.endeffector.EndEffector;
 import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands;
-import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants;
 import frc.trigon.robot.subsystems.intake.Intake;
 import frc.trigon.robot.subsystems.intake.IntakeCommands;
 import frc.trigon.robot.subsystems.intake.IntakeConstants;
@@ -45,7 +39,7 @@
 
 public class RobotContainer {
     public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator(
-            CameraConstants.INTAKE_SIDE_REEF_TAG_CAMERA,
+            CameraConstants.FRONT_REEF_TAG_CAMERA,
             CameraConstants.LEFT_REEF_TAG_CAMERA,
             CameraConstants.RIGHT_REEF_TAG_CAMERA
     );
@@ -80,13 +74,14 @@ public Command getAutonomousCommand() {
     private void configureBindings() {
         bindDefaultCommands();
         bindControllerCommands();
+//        configureSysIDBindings(ARM_ELEVATOR);
     }
 
     private void bindDefaultCommands() {
         SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand());
         ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand());
-        CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST));
-        END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST));
+        CLIMBER.setDefaultCommand(ClimberCommands.getDefaultCommand());
+        END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getDefaultCommand());
         INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST));
         TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST));
     }
@@ -118,7 +113,15 @@ private void bindControllerCommands() {
         OperatorConstants.SPAWN_CORAL_IN_SIMULATION_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())));
         OperatorConstants.FLIP_ARM_TRIGGER.onTrue(new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE));
         OperatorConstants.LOLLIPOP_ALGAE_TOGGLE_TRIGGER.onTrue(new InstantCommand(AlgaeManipulationCommands::toggleLollipopCollection));
+        OperatorConstants.SHOULD_LOAD_CORAL_TOGGLE_TRIGGER.onTrue(GeneralCommands.getToggleShouldLoadCoralCommand());
+        OperatorConstants.SHOULD_MANIPULATE_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldManipulateCoralAutonomouslyCommand());
+        OperatorConstants.SHOULD_COLLECT_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldCollectCoralAtonomouslyCommand());
+        OperatorConstants.SHOULD_SCORE_CORAL_ATONOMOUSLY_TRIGGER.onTrue(GeneralCommands.getToggleShouldScoreCoralAtonomouslyCommand());
         OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand());
+//        OperatorConstants.DEBUGGING_TRIGGER.whileTrue(ClimberCommands.getDebuggingCommand());
+        OperatorConstants.RESET_CLIMBER_POSITION_TRIGGER.whileTrue(ClimberCommands.resetClimberPositionCommand().ignoringDisable(true));
+        OperatorConstants.RESET_INTAKE_POSITION_TRIGGER.whileTrue(IntakeCommands.resetIntakePositionCommand().ignoringDisable(true));
+        OperatorConstants.RESET_ELEVATOR_POSITION_TRIGGER.whileTrue(ArmElevatorCommands.resetElevatorPositionCommand().ignoringDisable(true));
     }
 
     private void configureSysIDBindings(MotorSubsystem subsystem) {
@@ -161,7 +164,11 @@ private void buildAutoChooser() {
 
         addCommandsToChooser(
                 AutonomousCommands.getFloorAutonomousCommand(true),
-                AutonomousCommands.getFloorAutonomousCommand(false)
+                AutonomousCommands.getFloorAutonomousCommand(false),
+                AutonomousCommands.getFloorAutonomousCommand(true, FieldConstants.ReefClockPosition.REEF_2_OCLOCK, FieldConstants.ReefClockPosition.REEF_4_OCLOCK),
+                AutonomousCommands.getFloorAutonomousCommand(false, FieldConstants.ReefClockPosition.REEF_8_OCLOCK, FieldConstants.ReefClockPosition.REEF_10_OCLOCK),
+                AutonomousCommands.getFloorAutonomousCommand(true, FieldConstants.ReefClockPosition.REEF_2_OCLOCK, FieldConstants.ReefClockPosition.REEF_4_OCLOCK, FieldConstants.ReefClockPosition.REEF_6_OCLOCK),
+                AutonomousCommands.getFloorAutonomousCommand(false, FieldConstants.ReefClockPosition.REEF_8_OCLOCK, FieldConstants.ReefClockPosition.REEF_10_OCLOCK, FieldConstants.ReefClockPosition.REEF_6_OCLOCK)
         );
     }
 

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

@@ -23,13 +23,13 @@ public class IntakeAssistCommand extends ParallelCommandGroup {
     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)),
+            new ProfiledPIDController(0.7, 0, 0, new TrapezoidProfile.Constraints(2.65, 4.5)),
             Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
                     new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
-                    new ProfiledPIDController(0.3, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 5.5)),
+                    new ProfiledPIDController(0.25, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 4)),
             THETA_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
                     new ProfiledPIDController(0.4, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
-                    new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5));
+                    new ProfiledPIDController(0.6, 0, 0, new TrapezoidProfile.Constraints(2.8, 4.5));
     private Translation2d distanceFromTrackedGamePiece;
 
     /**
@@ -96,8 +96,8 @@ private static Translation2d calculateTranslationPower(AssistMode assistMode, Tr
         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);
+        if (assistMode.isAlternate)
+            return calculateAlternateAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput);
         return calculateNormalAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput, intakeAssistScalar);
     }
 
@@ -115,14 +115,13 @@ private static double calculateThetaPower(AssistMode assistMode, Translation2d d
         return pow;
     }
 
-    private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) {
-        final double pidScalar = Math.cbrt(joystickValue.getNorm());
+    private static Translation2d calculateAlternateAssistTranslationPower(AssistMode assistMode, Translation2d joystickValue, double xPIDOutput, double yPIDOutput) {
         final double
-                xJoystickPower = Math.cbrt(joystickValue.getX()),
-                yJoystickPower = Math.cbrt(joystickValue.getY());
+                xJoystickPower = joystickValue.getX(),
+                yJoystickPower = joystickValue.getY();
         final double
-                xPower = calculateAlternateAssistPower(xPIDOutput, pidScalar, xJoystickPower),
-                yPower = calculateAlternateAssistPower(yPIDOutput, pidScalar, yJoystickPower);
+                xPower = assistMode.shouldAssistX ? calculateAlternateAssistPower(xPIDOutput, xJoystickPower) : xJoystickPower,
+                yPower = assistMode.shouldAssistY ? calculateAlternateAssistPower(yPIDOutput, yJoystickPower) : yJoystickPower;
 
         return new Translation2d(xPower, yPower);
     }
@@ -143,17 +142,17 @@ private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2
                 pidOutput = clampToOutputRange(THETA_PID_CONTROLLER.calculate(thetaOffset.getRadians())),
                 joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX();
 
-        if (assistMode.equals(AssistMode.ALTERNATE_ASSIST))
-            return calculateAlternateAssistPower(pidOutput, joystickValue, joystickValue);
+        if (assistMode.isAlternate)
+            return calculateAlternateAssistPower(pidOutput, joystickValue);
         return calculateNormalAssistPower(pidOutput, joystickValue, intakeAssistScalar);
     }
 
     private static double clampToOutputRange(double value) {
         return MathUtil.clamp(value, -1, 1);
     }
 
-    private static double calculateAlternateAssistPower(double pidOutput, double pidScalar, double joystickPower) {
-        return pidOutput * (1 - Math.abs(pidScalar)) + joystickPower;
+    private static double calculateAlternateAssistPower(double pidOutput, double joystickPower) {
+        return pidOutput * (1 - Math.abs(joystickPower)) + joystickPower;
     }
 
     private static double calculateNormalAssistPower(double pidOutput, double joystickPower, double scalar) {
@@ -162,6 +161,7 @@ private static double calculateNormalAssistPower(double pidOutput, double joysti
 
     private static void resetPIDControllers(Translation2d distanceFromTrackedGamePiece) {
         X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond);
+        X_PID_CONTROLLER.setGoal(-0.15);
         Y_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getY(), RobotContainer.SWERVE.getSelfRelativeVelocity().vyMetersPerSecond);
         THETA_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg).unaryMinus().getRadians(), RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond);
     }
@@ -173,29 +173,35 @@ public enum AssistMode {
         /**
          * An alternate method for assisting the intake where the pid output is scaled down the more input the driver gives.
          */
-        ALTERNATE_ASSIST(true, true, true),
+        ALTERNATE_ASSIST(true, true, true, true),
+        /**
+         * An alternate method for assisting the intake where the pid output is scaled down the more input the driver gives.
+         */
+        ALTERNATE_ALIGN(false, true, true, true),
         /**
          * 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),
+        FULL_ASSIST(true, true, true, false),
         /**
          * 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),
+        ALIGN_ASSIST(false, true, true, false),
         /**
          * 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);
+        ASSIST_ROTATION(false, false, true, false);
 
         final boolean
                 shouldAssistX,
                 shouldAssistY,
-                shouldAssistTheta;
+                shouldAssistTheta,
+                isAlternate;
 
-        AssistMode(boolean shouldAssistX, boolean shouldAssistY, boolean shouldAssistTheta) {
+        AssistMode(boolean shouldAssistX, boolean shouldAssistY, boolean shouldAssistTheta, boolean isAlternate) {
             this.shouldAssistX = shouldAssistX;
             this.shouldAssistY = shouldAssistY;
             this.shouldAssistTheta = shouldAssistTheta;
+            this.isAlternate = isAlternate;
         }
     }
 }

src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java

@@ -49,7 +49,7 @@ public static Command getFloorAlgaeCollectionCommand() {
         return new SequentialCommandGroup(
                 GeneralCommands.getResetFlipArmOverrideCommand(),
                 CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece),
-                getInitiateFloorAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece),
+                getInitiateFloorAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.1))),
                 new InstantCommand(() -> {
                     IS_HOLDING_ALGAE = true;
                     SHOULD_COLLECT_FROM_LOLLIPOP = false;
@@ -64,7 +64,7 @@ public static Command getReefAlgaeCollectionCommand() {
         return new SequentialCommandGroup(
                 GeneralCommands.getResetFlipArmOverrideCommand(),
                 CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece),
-                getInitiateReefAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece),
+                getInitiateReefAlgaeCollectionCommand().raceWith(new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece).andThen(new WaitCommand(0.1))),
                 new InstantCommand(() -> IS_HOLDING_ALGAE = true),
                 GeneralCommands.getResetFlipArmOverrideCommand(),
                 getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand())
@@ -110,6 +110,13 @@ private static Command getHoldAlgaeCommand() {
     }
 
     private static Command getScoreInNetCommand() {
+        return new ParallelRaceGroup(
+                GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_NET, false, AlgaeManipulationCommands::shouldReverseNetScore),
+                GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER)
+        );
+    }
+
+    private static Command getAutonomouslyScoreInNetCommand() {
         return new ParallelRaceGroup(
                 GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_NET, false, AlgaeManipulationCommands::shouldReverseNetScore),
                 GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER),
@@ -119,7 +126,14 @@ private static Command getScoreInNetCommand() {
 
     private static Command getScoreInProcessorCommand() {
         return new ParallelCommandGroup(
-                GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_PROCESSOR, false),
+                ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.SCORE_PROCESSOR),
+                GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER)
+        ).finallyDo(GeneralCommands.getFieldRelativeDriveCommand()::schedule);
+    }
+
+    private static Command getAtonomouslyScoreInProcessorCommand() {
+        return new ParallelCommandGroup(
+                ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.SCORE_PROCESSOR),
                 GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER),
                 getDriveToProcessorCommand()
         ).finallyDo(GeneralCommands.getFieldRelativeDriveCommand()::schedule);
@@ -181,7 +195,7 @@ private static Command getCollectAlgaeFromLollipopSequenceCommand() {
                         .onlyIf(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE)
                         .until(() -> !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE)
                         .repeatedly()
-        );
+        ).finallyDo(() -> SHOULD_COLLECT_FROM_LOLLIPOP = false);
     }
 
     private static Command getCollectAlgaeFromFloorSequenceCommand() {