From fe832a2bc367c3b75fe5be2c7282a0059651d629 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sat, 4 Apr 2026 13:03:19 -0600 Subject: [PATCH] Intake Reference added --- .../autos/X. LeftTrench-Neutral-Shoot.auto | 4 +-- .../autos/X. RightTrench-Neutral-Shoot.auto | 6 ++++ .../paths/LeftTrench-Neutral-Shoot.path | 4 +-- .../java/frc4388/robot/RobotContainer.java | 6 +++- .../robot/commands/waitIntakeReference.java | 36 +++++++++++++++++++ .../frc4388/robot/commands/waitSupplier.java | 36 +++++++++++++++++++ .../robot/subsystems/intake/Intake.java | 4 +++ 7 files changed, 91 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/waitIntakeReference.java create mode 100644 src/main/java/frc4388/robot/commands/waitSupplier.java diff --git a/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto b/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto index 3230788..35e60ec 100644 --- a/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto +++ b/src/main/deploy/pathplanner/autos/X. LeftTrench-Neutral-Shoot.auto @@ -11,9 +11,9 @@ } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 0 + "name": "Intake Reference" } }, { diff --git a/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto b/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto index 1ca254e..51730f9 100644 --- a/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto +++ b/src/main/deploy/pathplanner/autos/X. RightTrench-Neutral-Shoot.auto @@ -10,6 +10,12 @@ "name": "Intake Extended" } }, + { + "type": "named", + "data": { + "name": "Intake Reference" + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path b/src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path index 679eeb1..8705985 100644 --- a/src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path +++ b/src/main/deploy/pathplanner/paths/LeftTrench-Neutral-Shoot.path @@ -121,11 +121,11 @@ { "fieldPosition": { "x": 0.0, - "y": 7.0 + "y": 7.4 }, "rotationOffset": 180.0, "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 0.7353266888150656, + "maxWaypointRelativePos": 0.6821705426356636, "name": "Point Towards Zone" } ], diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6a173c6..f149d68 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -36,6 +36,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc4388.robot.commands.waitSupplier; import frc4388.robot.commands.Swerve.StayInPosition; import frc4388.robot.commands.alignment.AutoAlign; import frc4388.robot.constants.Constants; @@ -137,6 +138,9 @@ public class RobotContainer { new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake) ); + private Command WaitIntakeReference = + new WaitUntilCommand(m_robotIntake::intakeAtReference); + private Command RobotShootDriving = new SequentialCommandGroup( new RunCommand(() -> m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION, ShooterConstants.AIM_LEAD_TIME.get(), FieldPositions.HUB_POSITION) @@ -186,7 +190,7 @@ public class RobotContainer { // NamedCommands.registerCommand("Lidar Intake", LidarIntake); NamedCommands.registerCommand("Intake Extended", IntakeExtended); NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving); - + NamedCommands.registerCommand("Intake Reference", WaitIntakeReference); NamedCommands.registerCommand("WaitShooter", new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed)); NamedCommands.registerCommand("AllowShooting", new InstantCommand(() -> m_robotShooter.allowShooting(), m_robotShooter)); NamedCommands.registerCommand("DenyShooting", new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter)); diff --git a/src/main/java/frc4388/robot/commands/waitIntakeReference.java b/src/main/java/frc4388/robot/commands/waitIntakeReference.java new file mode 100644 index 0000000..4f5660e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/waitIntakeReference.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.intake.Intake; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class waitIntakeReference extends Command { + /** Creates a new waitIntakeReference. */ + private Intake intake; + public waitIntakeReference(Intake intake) { + this.intake = intake; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return intake.intakeAtReference(); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/waitSupplier.java b/src/main/java/frc4388/robot/commands/waitSupplier.java new file mode 100644 index 0000000..f4cd5a6 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/waitSupplier.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class waitSupplier extends Command { + /** Creates a new waitSupplier. */ + private final Supplier truth; + public waitSupplier(Supplier truth) { + this.truth = truth; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return truth.get(); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 78c6e24..0df9a98 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -74,6 +74,10 @@ public class Intake extends SubsystemBase { return state.rollerTargetOutput; } + public boolean intakeAtReference() { + return state.extendedSoftLimit; + } + public double getRollerSpeed() { return state.rollerOutput; }