From 1d55ace5dee583fdf397634eca8782c6e9c0984d Mon Sep 17 00:00:00 2001 From: Trigon5990 Date: Sun, 22 Feb 2026 21:01:55 +0200 Subject: [PATCH 1/3] magnivish --- .../autonomous/AutonomousGenerator.java | 1 + .../autonomous/GeneralAutonomousCommands.java | 31 +++++++++++++++++-- 2 files changed, 29 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java index 259b745..86b7ce4 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java @@ -78,6 +78,7 @@ private static void log() { Logger.recordOutput("Autonomous/ShouldStartDrivingToClimb", shouldStartDrivingToClimb()); Logger.recordOutput("Autonomous/IsRight", SafeAutonomousDriveCommands.isRight()); Logger.recordOutput("Autonomous/IsInAllianceZone", SafeAutonomousDriveCommands.isInAllianceZone()); + Logger.recordOutput("Autonomous/isInTrench", GeneralAutonomousCommands.isInTrench()); } private static boolean shouldStartDrivingToClimb() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java index fcf52bd..b77ae20 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java @@ -2,6 +2,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto; 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.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.*; @@ -20,11 +21,13 @@ import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; import frc.trigon.robot.subsystems.hood.HoodCommands; +import frc.trigon.robot.subsystems.hood.HoodConstants; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import frc.trigon.robot.subsystems.turret.TurretCommands; import org.json.simple.parser.ParseException; +import org.littletonrobotics.junction.Logger; import java.io.IOException; import java.util.function.Supplier; @@ -128,12 +131,30 @@ private static Command getClimbToL1Command(Supplier climbPositi private static Command getShootAtHubWhileDrivingCommand() { return GeneralCommands.getContinuousConditionalCommand( - ShootingCommands.getShootAtHubCommand(), - getPrepareForShootingCommand(), - SafeAutonomousDriveCommands::isInAllianceZone + getPrepareForShootingWithoutHoodCommand().alongWith(new RunCommand(() -> Logger.recordOutput("Autonomous/magniv", "UNDER_DA_TRENCH"))), + GeneralCommands.getContinuousConditionalCommand( + ShootingCommands.getShootAtHubCommand().alongWith(new RunCommand(() -> Logger.recordOutput("Autonomous/magniv", "HUB"))), + getPrepareForShootingCommand().alongWith(new RunCommand(() -> Logger.recordOutput("Autonomous/magniv", "OVER_DA_TRENCH"))), + SafeAutonomousDriveCommands::isInAllianceZone + ), + GeneralAutonomousCommands::isInTrench ); } + public static boolean isInTrench() { + final Pose2d currentRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + double x1 = (isAngleCloserTo180(currentRobotPose.getRotation()) && !Flippable.isRedAlliance()) || (!isAngleCloserTo180(currentRobotPose.getRotation()) && Flippable.isRedAlliance()) ? 4.25 : 4.1; + double x2 = (isAngleCloserTo180(currentRobotPose.getRotation()) && !Flippable.isRedAlliance()) || (!isAngleCloserTo180(currentRobotPose.getRotation()) && Flippable.isRedAlliance()) ? 5.4 : 5.6; + x1 = Flippable.isRedAlliance() ? FieldConstants.FIELD_LENGTH_METERS - x1 : x1; + x2 = Flippable.isRedAlliance() ? FieldConstants.FIELD_LENGTH_METERS - x2 : x2; + return currentRobotPose.getX() > x1 && currentRobotPose.getX() < x2 + || currentRobotPose.getX() < x1 && currentRobotPose.getX() > x2; + } + + private static boolean isAngleCloserTo180(Rotation2d angle) { + return Math.abs(angle.getDegrees()) > 90; + } + private static Command getDriveToFuelInNeutralZoneCommand(boolean shootPreload, double timeout, boolean shouldWaitUntilAtPose, FlippablePose2d targetPose, boolean intakeSlowly) { return new SequentialCommandGroup( SafeAutonomousDriveCommands.getSafeDriveToPoseCommand( @@ -164,6 +185,10 @@ private static Command getPrepareForShootingCommand() { ); } + private static Command getPrepareForShootingWithoutHoodCommand() { + return HoodCommands.getSetTargetAngleCommand(() -> Rotation2d.fromDegrees(87)); + } + private static Command getAimWithTargetShootingState(Supplier targetShootingState) { return new ParallelCommandGroup( TurretCommands.getSetTargetFieldRelativeAngleCommand(() -> targetShootingState.get().targetFieldRelativeYaw()), From 1bee66cd9b6e18c4de2b4f78b5fb07a34d13a978 Mon Sep 17 00:00:00 2001 From: Trigon5990 Date: Sun, 22 Feb 2026 21:03:33 +0200 Subject: [PATCH 2/3] Update GeneralAutonomousCommands.java Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../autonomous/GeneralAutonomousCommands.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java index b77ae20..546ea82 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java @@ -143,12 +143,12 @@ private static Command getShootAtHubWhileDrivingCommand() { public static boolean isInTrench() { final Pose2d currentRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - double x1 = (isAngleCloserTo180(currentRobotPose.getRotation()) && !Flippable.isRedAlliance()) || (!isAngleCloserTo180(currentRobotPose.getRotation()) && Flippable.isRedAlliance()) ? 4.25 : 4.1; - double x2 = (isAngleCloserTo180(currentRobotPose.getRotation()) && !Flippable.isRedAlliance()) || (!isAngleCloserTo180(currentRobotPose.getRotation()) && Flippable.isRedAlliance()) ? 5.4 : 5.6; - x1 = Flippable.isRedAlliance() ? FieldConstants.FIELD_LENGTH_METERS - x1 : x1; - x2 = Flippable.isRedAlliance() ? FieldConstants.FIELD_LENGTH_METERS - x2 : x2; - return currentRobotPose.getX() > x1 && currentRobotPose.getX() < x2 - || currentRobotPose.getX() < x1 && currentRobotPose.getX() > x2; + double entryXFromAllianceZone = (isAngleCloserTo180(currentRobotPose.getRotation()) && !Flippable.isRedAlliance()) || (!isAngleCloserTo180(currentRobotPose.getRotation()) && Flippable.isRedAlliance()) ? 4.25 : 4.1; + double entryXFromNeutralZone = (isAngleCloserTo180(currentRobotPose.getRotation()) && !Flippable.isRedAlliance()) || (!isAngleCloserTo180(currentRobotPose.getRotation()) && Flippable.isRedAlliance()) ? 5.4 : 5.6; + entryXFromAllianceZone = Flippable.isRedAlliance() ? FieldConstants.FIELD_LENGTH_METERS - entryXFromAllianceZone : entryXFromAllianceZone; + entryXFromNeutralZone = Flippable.isRedAlliance() ? FieldConstants.FIELD_LENGTH_METERS - entryXFromNeutralZone : entryXFromNeutralZone; + return currentRobotPose.getX() > entryXFromAllianceZone && currentRobotPose.getX() < entryXFromNeutralZone + || currentRobotPose.getX() < entryXFromAllianceZone && currentRobotPose.getX() > entryXFromNeutralZone; } private static boolean isAngleCloserTo180(Rotation2d angle) { From c9e042d27dca6a539d3f91b48cc8744c0c4ffe30 Mon Sep 17 00:00:00 2001 From: Trigon5990 Date: Sun, 22 Feb 2026 21:35:23 +0200 Subject: [PATCH 3/3] cleaning Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../autonomous/AutonomousGenerator.java | 2 +- .../autonomous/GeneralAutonomousCommands.java | 13 ++++++------- .../trigon/robot/constants/AutonomousConstants.java | 2 +- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java index 86b7ce4..ec27e5b 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java @@ -78,7 +78,7 @@ private static void log() { Logger.recordOutput("Autonomous/ShouldStartDrivingToClimb", shouldStartDrivingToClimb()); Logger.recordOutput("Autonomous/IsRight", SafeAutonomousDriveCommands.isRight()); Logger.recordOutput("Autonomous/IsInAllianceZone", SafeAutonomousDriveCommands.isInAllianceZone()); - Logger.recordOutput("Autonomous/isInTrench", GeneralAutonomousCommands.isInTrench()); + Logger.recordOutput("Autonomous/IsInTrench", GeneralAutonomousCommands.isInTrench()); } private static boolean shouldStartDrivingToClimb() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java index 546ea82..2ea9e54 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java @@ -27,7 +27,6 @@ import frc.trigon.robot.subsystems.swerve.SwerveCommands; import frc.trigon.robot.subsystems.turret.TurretCommands; import org.json.simple.parser.ParseException; -import org.littletonrobotics.junction.Logger; import java.io.IOException; import java.util.function.Supplier; @@ -131,10 +130,10 @@ private static Command getClimbToL1Command(Supplier climbPositi private static Command getShootAtHubWhileDrivingCommand() { return GeneralCommands.getContinuousConditionalCommand( - getPrepareForShootingWithoutHoodCommand().alongWith(new RunCommand(() -> Logger.recordOutput("Autonomous/magniv", "UNDER_DA_TRENCH"))), + getPrepareForShootingWithoutHoodCommand(), GeneralCommands.getContinuousConditionalCommand( - ShootingCommands.getShootAtHubCommand().alongWith(new RunCommand(() -> Logger.recordOutput("Autonomous/magniv", "HUB"))), - getPrepareForShootingCommand().alongWith(new RunCommand(() -> Logger.recordOutput("Autonomous/magniv", "OVER_DA_TRENCH"))), + ShootingCommands.getShootAtHubCommand(), + getPrepareForShootingCommand(), SafeAutonomousDriveCommands::isInAllianceZone ), GeneralAutonomousCommands::isInTrench @@ -143,8 +142,8 @@ private static Command getShootAtHubWhileDrivingCommand() { public static boolean isInTrench() { final Pose2d currentRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - double entryXFromAllianceZone = (isAngleCloserTo180(currentRobotPose.getRotation()) && !Flippable.isRedAlliance()) || (!isAngleCloserTo180(currentRobotPose.getRotation()) && Flippable.isRedAlliance()) ? 4.25 : 4.1; - double entryXFromNeutralZone = (isAngleCloserTo180(currentRobotPose.getRotation()) && !Flippable.isRedAlliance()) || (!isAngleCloserTo180(currentRobotPose.getRotation()) && Flippable.isRedAlliance()) ? 5.4 : 5.6; + double entryXFromAllianceZone = isAngleCloserTo180(currentRobotPose.getRotation()) ^ Flippable.isRedAlliance() ? 4.25 : 4.1; + double entryXFromNeutralZone = isAngleCloserTo180(currentRobotPose.getRotation()) ^ Flippable.isRedAlliance() ? 5.4 : 5.6; entryXFromAllianceZone = Flippable.isRedAlliance() ? FieldConstants.FIELD_LENGTH_METERS - entryXFromAllianceZone : entryXFromAllianceZone; entryXFromNeutralZone = Flippable.isRedAlliance() ? FieldConstants.FIELD_LENGTH_METERS - entryXFromNeutralZone : entryXFromNeutralZone; return currentRobotPose.getX() > entryXFromAllianceZone && currentRobotPose.getX() < entryXFromNeutralZone @@ -186,7 +185,7 @@ private static Command getPrepareForShootingCommand() { } private static Command getPrepareForShootingWithoutHoodCommand() { - return HoodCommands.getSetTargetAngleCommand(() -> Rotation2d.fromDegrees(87)); + return HoodCommands.getRestCommand(); } private static Command getAimWithTargetShootingState(Supplier targetShootingState) { diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 3daa6c4..12feccd 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -41,7 +41,7 @@ public class AutonomousConstants { AUTONOMOUS_TIME_SECONDS = 20, DEPOT_COLLECTION_TIMEOUT_SECONDS = 4, NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS = 1, - SCORING_TIMEOUT_SECONDS = 3.5, + SCORING_TIMEOUT_SECONDS = 3.7, ESTIMATED_CLIMBING_TIME_SECONDS = 3; public static final double ROBOT_AVERAGE_SPEED_METERS_PER_SECOND = 1,