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..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,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..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 @@ -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,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; @@ -128,12 +130,30 @@ private static Command getClimbToL1Command(Supplier 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( @@ -164,6 +184,10 @@ private static Command getPrepareForShootingCommand() { ); } + private static Command getPrepareForShootingWithoutHoodCommand() { + return HoodCommands.getRestCommand(); + } + private static Command getAimWithTargetShootingState(Supplier targetShootingState) { return new ParallelCommandGroup( TurretCommands.getSetTargetFieldRelativeAngleCommand(() -> targetShootingState.get().targetFieldRelativeYaw()), 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,