Skip to content

No Hitting Trench In Autonomous #50

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Trigon5990 merged 3 commits into from
Feb 22, 2026
Merged

No Hitting Trench In Autonomous #50

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1d55ace
magnivish
Trigon5990 Feb 22, 2026
1bee66c
Update GeneralAutonomousCommands.java
Trigon5990 Feb 22, 2026
c9e042d
cleaning
Trigon5990 Feb 22, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/AutonomousGenerator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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() {
Expand Down
30 changes: 27 additions & 3 deletions ...java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.*;
Expand All @@ -20,6 +21,7 @@
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;
Expand Down Expand Up @@ -128,12 +130,30 @@ private static Command getClimbToL1Command(Supplier<FlippablePose2d> climbPositi

private static Command getShootAtHubWhileDrivingCommand() {
return GeneralCommands.getContinuousConditionalCommand(
ShootingCommands.getShootAtHubCommand(),
getPrepareForShootingCommand(),
SafeAutonomousDriveCommands::isInAllianceZone
getPrepareForShootingWithoutHoodCommand(),
GeneralCommands.getContinuousConditionalCommand(
ShootingCommands.getShootAtHubCommand(),
getPrepareForShootingCommand(),
SafeAutonomousDriveCommands::isInAllianceZone
),
GeneralAutonomousCommands::isInTrench
);
}

public static boolean isInTrench() {
final Pose2d currentRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
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
|| currentRobotPose.getX() < entryXFromAllianceZone && currentRobotPose.getX() > entryXFromNeutralZone;
}

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(
Expand Down Expand Up @@ -164,6 +184,10 @@ private static Command getPrepareForShootingCommand() {
);
}

private static Command getPrepareForShootingWithoutHoodCommand() {
return HoodCommands.getRestCommand();
}

private static Command getAimWithTargetShootingState(Supplier<ShootingState> targetShootingState) {
return new ParallelCommandGroup(
TurretCommands.getSetTargetFieldRelativeAngleCommand(() -> targetShootingState.get().targetFieldRelativeYaw()),
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/trigon/robot/constants/AutonomousConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
Loading