From 312f1e6966a8a731d6b82ec10fe29a5a425eacb6 Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared-K1-10@users.noreply.github.com> Date: Fri, 12 Jun 2026 21:12:03 -0700 Subject: [PATCH 1/9] Created timer for robot agitation Co-Authored-By: BrodyKarr <145169276+BrodyKarr@users.noreply.github.com> Co-Authored-By: Justinpham17386769 <244249272+Justinpham17386769@users.noreply.github.com> Co-Authored-By: jayden mendoza <244560677+jaymendo670-png@users.noreply.github.com> --- .../frc/robot/commands/states/Shooting.java | 26 ++++++++++++++++++- .../java/frc/robot/constants/ConstMotion.java | 3 +++ .../java/frc/robot/subsystems/Motion.java | 11 ++++++++ 3 files changed, 39 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/states/Shooting.java b/src/main/java/frc/robot/commands/states/Shooting.java index 61a21b1..c71717c 100644 --- a/src/main/java/frc/robot/commands/states/Shooting.java +++ b/src/main/java/frc/robot/commands/states/Shooting.java @@ -4,6 +4,9 @@ package frc.robot.commands.states; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; import frc.robot.constants.ConstMotion; @@ -12,8 +15,13 @@ public class Shooting extends Command { /** Creates a new Shooting. */ + + private final Timer intakeAgitationTimer = new Timer(); + private RobotState previousState;RobotBase + public Shooting() { + // Use addRequirements() here to declare subsystem dependencies. addRequirements(RobotContainer.stateMachineInstance); } @@ -29,6 +37,21 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + intakeAgitationTimer.start(); + if (!RobotContainer.motionInstance.isIntakePivotAtAngle(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE, + ConstMotion.INTAKE_PIVOT_TOLERANCE) + && intakeAgitationTimer.hasElapsed(ConstMotion.INTAKE_PIVOT_AGITATION_TIME)) { + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE); + intakeAgitationTimer.stop(); + intakeAgitationTimer.restart(); + intakeAgitationTimer.start(); + + } else if (intakeAgitationTimer.hasElapsed((ConstMotion.INTAKE_PIVOT_AGITATION_TIME))) { + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.DEPLOY_INTAKE_PIVOT_ANGLE); + intakeAgitationTimer.stop(); + intakeAgitationTimer.restart(); + } + } // Called once the command ends or is interrupted. @@ -38,7 +61,8 @@ public void end(boolean interrupted) { RobotContainer.rotorsInstance.setTransferRollersPercentOutput(ConstRotors.STOP); RobotContainer.rotorsInstance.setSerializerRollersPercentOutput(ConstRotors.STOP); RobotContainer.motionInstance.setHoodPivotAngle(ConstMotion.HOOD_PIVOT_ANGLE_RETRACT); - + RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.DEPLOY_INTAKE_PIVOT_ANGLE); + intakeAgitationTimer.stop(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index d367b3f..118cfc7 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -15,9 +15,12 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Time; /** Add your docs here. */ public class ConstMotion { + public static final Angle INTAKE_PIVOT_TOLERANCE = Degrees.of(1.0); + public static final Time INTAKE_PIVOT_AGITATION_TIME = Units.Seconds.of(1.0 / 3.0); public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final Angle DEPLOY_INTAKE_PIVOT_ANGLE = Degrees.of(125); diff --git a/src/main/java/frc/robot/subsystems/Motion.java b/src/main/java/frc/robot/subsystems/Motion.java index 39c82b0..622619a 100644 --- a/src/main/java/frc/robot/subsystems/Motion.java +++ b/src/main/java/frc/robot/subsystems/Motion.java @@ -91,6 +91,17 @@ public boolean isIntakePivotAtAngle(Angle tolerance) { return intakePivotAtPosition; } + public boolean isIntakePivotAtAngle(Angle tol, Angle target) { + Angle lowerlim = target.minus(tol); + Angle upperlim = target.plus(tol); + + Angle intakePivotAngle = getIntakePivotAngle(); + + intakePivotAtPosition = intakePivotAngle.gte(lowerlim) + && intakePivotAngle.lte(upperlim); + return intakePivotAtPosition; + } + @Override public void periodic() { // This method will be called once per scheduler run From 9bace303f93f4bb9bb50d9648d675e8673c45842 Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared-K1-10@users.noreply.github.com> Date: Sat, 13 Jun 2026 13:26:43 -0700 Subject: [PATCH 2/9] robot agitation --- src/main/java/frc/robot/commands/states/Shooting.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/states/Shooting.java b/src/main/java/frc/robot/commands/states/Shooting.java index c71717c..bcdd8d8 100644 --- a/src/main/java/frc/robot/commands/states/Shooting.java +++ b/src/main/java/frc/robot/commands/states/Shooting.java @@ -17,11 +17,10 @@ public class Shooting extends Command { /** Creates a new Shooting. */ private final Timer intakeAgitationTimer = new Timer(); - private RobotState previousState;RobotBase + // private RobotState previousState;RobotBase public Shooting() { - // Use addRequirements() here to declare subsystem dependencies. addRequirements(RobotContainer.stateMachineInstance); } From 29a2c9d461777e30f62d5483d1ef3908c2b6b247 Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared-K1-10@users.noreply.github.com> Date: Sat, 13 Jun 2026 13:50:33 -0700 Subject: [PATCH 3/9] intake agitation --- src/main/java/frc/robot/commands/states/Shooting.java | 6 +++--- src/main/java/frc/robot/subsystems/Motion.java | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/states/Shooting.java b/src/main/java/frc/robot/commands/states/Shooting.java index bcdd8d8..3c25e1f 100644 --- a/src/main/java/frc/robot/commands/states/Shooting.java +++ b/src/main/java/frc/robot/commands/states/Shooting.java @@ -41,13 +41,13 @@ public void execute() { ConstMotion.INTAKE_PIVOT_TOLERANCE) && intakeAgitationTimer.hasElapsed(ConstMotion.INTAKE_PIVOT_AGITATION_TIME)) { RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE); - intakeAgitationTimer.stop(); + // intakeAgitationTimer.stop(); intakeAgitationTimer.restart(); - intakeAgitationTimer.start(); + // intakeAgitationTimer.start(); } else if (intakeAgitationTimer.hasElapsed((ConstMotion.INTAKE_PIVOT_AGITATION_TIME))) { RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.DEPLOY_INTAKE_PIVOT_ANGLE); - intakeAgitationTimer.stop(); + // intakeAgitationTimer.stop(); intakeAgitationTimer.restart(); } diff --git a/src/main/java/frc/robot/subsystems/Motion.java b/src/main/java/frc/robot/subsystems/Motion.java index 622619a..4de4939 100644 --- a/src/main/java/frc/robot/subsystems/Motion.java +++ b/src/main/java/frc/robot/subsystems/Motion.java @@ -80,7 +80,7 @@ public boolean isHoodPivotAtAngle(Angle tolerance) { return hoodAtPostion; } - public boolean isIntakePivotAtAngle(Angle tolerance) { + public boolean getIntakePivotAtAngle(Angle tolerance) { Angle lowerlim = lastDesiredIntakePivotAngle.minus(tolerance); Angle upperlim = lastDesiredIntakePivotAngle.plus(tolerance); From ebcfdabdf41c325f2114947e720fdf81c9ba96a1 Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared-K1-10@users.noreply.github.com> Date: Sat, 13 Jun 2026 13:51:04 -0700 Subject: [PATCH 4/9] intake agitation --- src/main/java/frc/robot/commands/states/Shooting.java | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/states/Shooting.java b/src/main/java/frc/robot/commands/states/Shooting.java index 3c25e1f..55bc1a1 100644 --- a/src/main/java/frc/robot/commands/states/Shooting.java +++ b/src/main/java/frc/robot/commands/states/Shooting.java @@ -31,23 +31,21 @@ public void initialize() { RobotContainer.stateMachineInstance.setRobotState((StateMachine.RobotState.SHOOTING)); RobotContainer.rotorsInstance.setTransferRollersSpeeds(ConstRotors.TRANSFER_ROLLERS_SPEED); RobotContainer.rotorsInstance.setSerializerRollersPercentOutput(ConstRotors.SERIALIZER_SHOOTING_SPEED); + intakeAgitationTimer.start(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - intakeAgitationTimer.start(); - if (!RobotContainer.motionInstance.isIntakePivotAtAngle(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE, + if (!RobotContainer.motionInstance.getIntakePivotAngle().isNear(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE, ConstMotion.INTAKE_PIVOT_TOLERANCE) && intakeAgitationTimer.hasElapsed(ConstMotion.INTAKE_PIVOT_AGITATION_TIME)) { RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE); - // intakeAgitationTimer.stop(); intakeAgitationTimer.restart(); - // intakeAgitationTimer.start(); } else if (intakeAgitationTimer.hasElapsed((ConstMotion.INTAKE_PIVOT_AGITATION_TIME))) { RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.DEPLOY_INTAKE_PIVOT_ANGLE); - // intakeAgitationTimer.stop(); intakeAgitationTimer.restart(); } From ed183ca5b3ae443e892cd720e508415c946766cd Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared-K1-10@users.noreply.github.com> Date: Sat, 13 Jun 2026 14:10:16 -0700 Subject: [PATCH 5/9] changed hasElasped to advancedIfElasped --- src/main/java/frc/robot/commands/states/Shooting.java | 4 ++-- src/main/java/frc/robot/constants/ConstMotion.java | 3 +-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/states/Shooting.java b/src/main/java/frc/robot/commands/states/Shooting.java index 55bc1a1..d461db1 100644 --- a/src/main/java/frc/robot/commands/states/Shooting.java +++ b/src/main/java/frc/robot/commands/states/Shooting.java @@ -40,11 +40,11 @@ public void initialize() { public void execute() { if (!RobotContainer.motionInstance.getIntakePivotAngle().isNear(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE, ConstMotion.INTAKE_PIVOT_TOLERANCE) - && intakeAgitationTimer.hasElapsed(ConstMotion.INTAKE_PIVOT_AGITATION_TIME)) { + && intakeAgitationTimer.advanceIfElapsed(ConstMotion.INTAKE_PIVOT_AGITATION_TIME)) { RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE); intakeAgitationTimer.restart(); - } else if (intakeAgitationTimer.hasElapsed((ConstMotion.INTAKE_PIVOT_AGITATION_TIME))) { + } else if (intakeAgitationTimer.advanceIfElapsed((ConstMotion.INTAKE_PIVOT_AGITATION_TIME))) { RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.DEPLOY_INTAKE_PIVOT_ANGLE); intakeAgitationTimer.restart(); } diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index 118cfc7..b86c800 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -15,12 +15,11 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Time; /** Add your docs here. */ public class ConstMotion { public static final Angle INTAKE_PIVOT_TOLERANCE = Degrees.of(1.0); - public static final Time INTAKE_PIVOT_AGITATION_TIME = Units.Seconds.of(1.0 / 3.0); + public static final double INTAKE_PIVOT_AGITATION_TIME = 1 / 3; public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final Angle DEPLOY_INTAKE_PIVOT_ANGLE = Degrees.of(125); From c9574d0ac320c16982a63156b3cb72a84fd2d53a Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared-K1-10@users.noreply.github.com> Date: Sat, 13 Jun 2026 14:11:46 -0700 Subject: [PATCH 6/9] removed unecessary comment --- src/main/java/frc/robot/commands/states/Shooting.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/states/Shooting.java b/src/main/java/frc/robot/commands/states/Shooting.java index d461db1..209d7e5 100644 --- a/src/main/java/frc/robot/commands/states/Shooting.java +++ b/src/main/java/frc/robot/commands/states/Shooting.java @@ -17,7 +17,6 @@ public class Shooting extends Command { /** Creates a new Shooting. */ private final Timer intakeAgitationTimer = new Timer(); - // private RobotState previousState;RobotBase public Shooting() { From 623cf3a9fd08c438f365f4f1fd58c4fe4c06559b Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared-K1-10@users.noreply.github.com> Date: Sat, 13 Jun 2026 14:34:36 -0700 Subject: [PATCH 7/9] changed back advancedIfElapsed --- .../frc/robot/commands/states/Shooting.java | 4 ++-- .../java/frc/robot/constants/ConstMotion.java | 3 ++- .../java/frc/robot/subsystems/Motion.java | 23 ------------------- 3 files changed, 4 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/commands/states/Shooting.java b/src/main/java/frc/robot/commands/states/Shooting.java index 209d7e5..95bc0da 100644 --- a/src/main/java/frc/robot/commands/states/Shooting.java +++ b/src/main/java/frc/robot/commands/states/Shooting.java @@ -39,11 +39,11 @@ public void initialize() { public void execute() { if (!RobotContainer.motionInstance.getIntakePivotAngle().isNear(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE, ConstMotion.INTAKE_PIVOT_TOLERANCE) - && intakeAgitationTimer.advanceIfElapsed(ConstMotion.INTAKE_PIVOT_AGITATION_TIME)) { + && intakeAgitationTimer.hasElapsed(ConstMotion.INTAKE_PIVOT_AGITATION_TIME)) { RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.RETRACT_INTAKE_PIVOT_ANGLE); intakeAgitationTimer.restart(); - } else if (intakeAgitationTimer.advanceIfElapsed((ConstMotion.INTAKE_PIVOT_AGITATION_TIME))) { + } else if (intakeAgitationTimer.hasElapsed((ConstMotion.INTAKE_PIVOT_AGITATION_TIME))) { RobotContainer.motionInstance.setIntakePivotAngle(ConstMotion.DEPLOY_INTAKE_PIVOT_ANGLE); intakeAgitationTimer.restart(); } diff --git a/src/main/java/frc/robot/constants/ConstMotion.java b/src/main/java/frc/robot/constants/ConstMotion.java index b86c800..118cfc7 100644 --- a/src/main/java/frc/robot/constants/ConstMotion.java +++ b/src/main/java/frc/robot/constants/ConstMotion.java @@ -15,11 +15,12 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Time; /** Add your docs here. */ public class ConstMotion { public static final Angle INTAKE_PIVOT_TOLERANCE = Degrees.of(1.0); - public static final double INTAKE_PIVOT_AGITATION_TIME = 1 / 3; + public static final Time INTAKE_PIVOT_AGITATION_TIME = Units.Seconds.of(1.0 / 3.0); public static final TalonFXConfiguration INTAKE_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final TalonFXConfiguration HOOD_PIVOT_CONFIGURATION = new TalonFXConfiguration(); public static final Angle DEPLOY_INTAKE_PIVOT_ANGLE = Degrees.of(125); diff --git a/src/main/java/frc/robot/subsystems/Motion.java b/src/main/java/frc/robot/subsystems/Motion.java index 4de4939..edf68b8 100644 --- a/src/main/java/frc/robot/subsystems/Motion.java +++ b/src/main/java/frc/robot/subsystems/Motion.java @@ -29,7 +29,6 @@ public class Motion extends SubsystemBase { Angle lastDesiredIntakePivotAngle = Degrees.zero(); private boolean hoodAtPostion = false; - private boolean intakePivotAtPosition = false; public Motion() { intakePivot.getConfigurator().apply(ConstMotion.INTAKE_PIVOT_CONFIGURATION); @@ -80,28 +79,6 @@ public boolean isHoodPivotAtAngle(Angle tolerance) { return hoodAtPostion; } - public boolean getIntakePivotAtAngle(Angle tolerance) { - Angle lowerlim = lastDesiredIntakePivotAngle.minus(tolerance); - Angle upperlim = lastDesiredIntakePivotAngle.plus(tolerance); - - Angle intakePivotAngle = getIntakePivotAngle(); - - intakePivotAtPosition = intakePivotAngle.gte(lowerlim) - && intakePivotAngle.lte(upperlim); - return intakePivotAtPosition; - } - - public boolean isIntakePivotAtAngle(Angle tol, Angle target) { - Angle lowerlim = target.minus(tol); - Angle upperlim = target.plus(tol); - - Angle intakePivotAngle = getIntakePivotAngle(); - - intakePivotAtPosition = intakePivotAngle.gte(lowerlim) - && intakePivotAngle.lte(upperlim); - return intakePivotAtPosition; - } - @Override public void periodic() { // This method will be called once per scheduler run From 89082df4d072c899d4c8d6538f3ceb325ee34472 Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared-K1-10@users.noreply.github.com> Date: Sat, 13 Jun 2026 14:37:25 -0700 Subject: [PATCH 8/9] removed unused imports in shooting --- src/main/java/frc/robot/commands/states/Shooting.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/states/Shooting.java b/src/main/java/frc/robot/commands/states/Shooting.java index 95bc0da..51e0886 100644 --- a/src/main/java/frc/robot/commands/states/Shooting.java +++ b/src/main/java/frc/robot/commands/states/Shooting.java @@ -4,8 +4,6 @@ package frc.robot.commands.states; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; From 7081779bac4991c898dbf2857227aa6f73684b2d Mon Sep 17 00:00:00 2001 From: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared-K1-10@users.noreply.github.com> Date: Sat, 13 Jun 2026 14:41:49 -0700 Subject: [PATCH 9/9] we replaced start with restart to ensure it starts at 0 seconds. --- src/main/java/frc/robot/commands/states/Shooting.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/states/Shooting.java b/src/main/java/frc/robot/commands/states/Shooting.java index 51e0886..ce62b1f 100644 --- a/src/main/java/frc/robot/commands/states/Shooting.java +++ b/src/main/java/frc/robot/commands/states/Shooting.java @@ -28,7 +28,7 @@ public void initialize() { RobotContainer.stateMachineInstance.setRobotState((StateMachine.RobotState.SHOOTING)); RobotContainer.rotorsInstance.setTransferRollersSpeeds(ConstRotors.TRANSFER_ROLLERS_SPEED); RobotContainer.rotorsInstance.setSerializerRollersPercentOutput(ConstRotors.SERIALIZER_SHOOTING_SPEED); - intakeAgitationTimer.start(); + intakeAgitationTimer.restart(); }