12 coral auto :)

Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com>

Author: ShmayaR

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

@@ -28,7 +28,7 @@ public class IntakeAssistCommand extends ParallelCommandGroup {
                     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)),
             THETA_PID_CONTROLLER = RobotHardwareStats.isSimulation() ?
-                    new ProfiledPIDController(0.2, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) :
+                    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));
     private Translation2d distanceFromTrackedGamePiece;
 
@@ -104,7 +104,15 @@ private static Translation2d calculateTranslationPower(AssistMode assistMode, Tr
     private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedGamePiece, double intakeAssistScalar) {
         if (distanceFromTrackedGamePiece == null || !assistMode.shouldAssistTheta)
             return 0;
-        return calculateThetaAssistPower(assistMode, distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg), intakeAssistScalar);
+        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) {
@@ -155,7 +163,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);
         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);
     }
 
     /**

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

@@ -63,8 +63,8 @@ public static Command getFloorAlgaeCollectionCommand() {
     public static Command getReefAlgaeCollectionCommand() {
         return new SequentialCommandGroup(
                 GeneralCommands.getResetFlipArmOverrideCommand(),
-                new InstantCommand(() -> IS_HOLDING_ALGAE = true),
                 CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece),
+                new InstantCommand(() -> IS_HOLDING_ALGAE = true),
                 getInitiateReefAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece),
                 GeneralCommands.getResetFlipArmOverrideCommand(),
                 getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand())
@@ -86,7 +86,7 @@ private static Command getAlignToReefCommand() {
                         () -> calculateClosestAlgaeCollectionPose().getRotation()
                 )
         ).raceWith(
-                new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece),
+                new WaitUntilCommand(() -> RobotContainer.END_EFFECTOR.hasGamePiece() && IS_HOLDING_ALGAE),
                 new WaitUntilCommand(OperatorConstants.STOP_REEF_ALGAE_ALIGN_TRIGGER)
         ).onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy();
     }

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

@@ -5,6 +5,7 @@
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.trigon.robot.RobotContainer;
+import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand;
 import frc.trigon.robot.constants.OperatorConstants;
 import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands;
 import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants;
@@ -26,8 +27,7 @@ public static Command getCoralCollectionCommand() {
                                 getLoadCoralCommand().schedule();
                         }
                 )
-        );
-        // new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE)
+        ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE));
     }
 
     public static Command getLoadCoralCommand() {
@@ -40,8 +40,8 @@ public static Command getLoadCoralCommand() {
     public static Command getUnloadCoralCommand() {
         return new ParallelCommandGroup(
                 ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.UNLOAD_CORAL),
-                EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.UNLOAD_CORAL)
-        ).until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece() && RobotContainer.INTAKE.hasCoral());
+                GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.UNLOAD_CORAL), () -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.UNLOAD_CORAL))
+        ).until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece());
     }
 
     static Command getIntakeSequenceCommand() {

src/main/java/frc/trigon/robot/constants/OperatorConstants.java

@@ -36,7 +36,7 @@ public class OperatorConstants {
             ROTATION_STICK_SPEED_DIVIDER = 1;
 
     public static final double INTAKE_ASSIST_SCALAR = 1;
-    public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST;
+    public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.FULL_ASSIST;
 
     public static final Trigger
             RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.povUp(),

src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java

@@ -336,7 +336,7 @@ public enum ArmElevatorState {
         REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7),
         REST_FOR_CLIMB(Rotation2d.fromDegrees(180), 0.603, null, false, 0.7),
         LOAD_CORAL(Rotation2d.fromDegrees(0), 0.5519, REST, true, 0.7),
-        UNLOAD_CORAL(Rotation2d.fromDegrees(0), 0.5519, null, false, 0.7),
+        UNLOAD_CORAL(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7),
         EJECT(Rotation2d.fromDegrees(60), 0.603, null, false, 0.7),
         SCORE_L1(Rotation2d.fromDegrees(70), 0.4, null, false, 1),
         SCORE_L2(Rotation2d.fromDegrees(90), 0.3, PREPARE_SCORE_L2, false, 1),