From 72c07a630ade54fd6a1535f0988bced99211d11c Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 3 Feb 2026 17:33:58 -0600 Subject: [PATCH 01/13] skeletonized part of intake subsystem --- src/main/java/frc/robot/Constants.java | 7 +++ .../frc/robot/subsystems/IntakeSubsystem.java | 54 +++++++++++++++++++ 2 files changed, 61 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/IntakeSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d5d42e0..80ba5a1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -148,4 +148,11 @@ public static final class VisionConstants { public static final class NumericalConstants { public static final double kEpsilon = 1e-6; } + + public static final class IntakeConstants { + public static final double kP = 1; + public static final double kI = 0; + public static final double kD = 1; + public static final int kMotorId = 1; + } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..1d06a3c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,54 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.*; + +import java.util.function.Supplier; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Velocity; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.Constants.IntakeConstants; + +public class IntakeSubsystem { + private final SparkMax m_IntakeMotor = new SparkMax(IntakeConstants.kMotorId, MotorType.kBrushless); + private final SparkMax m_DeployMotor = new SparkMax(IntakeConstants.kMotorId, MotorType.kBrushless); + + private final AbsoluteEncoder m_absoluteEncoder = m_DeployMotor.getAbsoluteEncoder(); + + private final SparkClosedLoopController m_IntakeClosedLoopController = m_IntakeMotor.getClosedLoopController(); + private final SparkClosedLoopController m_DeployClosedLoopController = m_IntakeMotor.getClosedLoopController(); + + private SparkMaxConfig m_PidConfig = new SparkMaxConfig(); + + public IntakeSubsystem() { + + m_PidConfig.closedLoop.p(IntakeConstants.kP).i(IntakeConstants.kI) + .d(IntakeConstants.kD); + m_PidConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + m_DeployMotor.configure(m_PidConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public void SpinIntake(AngularVelocity velocity) { + m_IntakeClosedLoopController.setSetpoint(velocity.in(RPM), ControlType.kVelocity); + } + + public void StopIntake() { + SpinIntake(RPM.of(0)); + } + + public void DeployPosition() { + } +} + + From 705df50a57b97c932abfa32acf9cab3b522038c1 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 5 Feb 2026 19:15:08 -0600 Subject: [PATCH 02/13] Add deploy and retract functions to intake --- src/main/java/frc/robot/Constants.java | 21 +++++++--- .../frc/robot/subsystems/IntakeSubsystem.java | 38 +++++++++---------- 2 files changed, 34 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 80ba5a1..9160a14 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,10 +14,16 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.DistanceUnit; +import edu.wpi.first.units.PerUnit; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Per; import edu.wpi.first.units.measure.Time; /** @@ -137,12 +143,11 @@ public static final class NeoMotorConstants { public static final class VisionConstants { - public static final Transform3d kRobotToCam - = new Transform3d(new Translation3d(0.5, 0.0, 0.5), - new Rotation3d(0, 0, 0)); + public static final Transform3d kRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), + new Rotation3d(0, 0, 0)); - public static final AprilTagFieldLayout kTagLayout - = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout + .loadField(AprilTagFields.kDefaultField); } public static final class NumericalConstants { @@ -152,7 +157,11 @@ public static final class NumericalConstants { public static final class IntakeConstants { public static final double kP = 1; public static final double kI = 0; - public static final double kD = 1; + public static final double kD = 0; public static final int kMotorId = 1; + + // Placeholder + public static final Angle kDeployRotations = Rotations.of(10.0); + public static final Angle kRetractRotations = Rotations.of(0.0); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 1d06a3c..9985c9e 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -2,8 +2,6 @@ import static edu.wpi.first.units.Units.*; -import java.util.function.Supplier; - import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; @@ -14,20 +12,15 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Velocity; -import edu.wpi.first.wpilibj2.command.RunCommand; import frc.robot.Constants.IntakeConstants; public class IntakeSubsystem { - private final SparkMax m_IntakeMotor = new SparkMax(IntakeConstants.kMotorId, MotorType.kBrushless); - private final SparkMax m_DeployMotor = new SparkMax(IntakeConstants.kMotorId, MotorType.kBrushless); - - private final AbsoluteEncoder m_absoluteEncoder = m_DeployMotor.getAbsoluteEncoder(); + private final SparkMax m_intakeMotor = new SparkMax(IntakeConstants.kMotorId, MotorType.kBrushless); + private final SparkMax m_deployMotor = new SparkMax(IntakeConstants.kMotorId, MotorType.kBrushless); - private final SparkClosedLoopController m_IntakeClosedLoopController = m_IntakeMotor.getClosedLoopController(); - private final SparkClosedLoopController m_DeployClosedLoopController = m_IntakeMotor.getClosedLoopController(); + private final SparkClosedLoopController m_intakeClosedLoopController = m_intakeMotor.getClosedLoopController(); + private final SparkClosedLoopController m_deployClosedLoopController = m_intakeMotor.getClosedLoopController(); private SparkMaxConfig m_PidConfig = new SparkMaxConfig(); @@ -36,19 +29,26 @@ public IntakeSubsystem() { m_PidConfig.closedLoop.p(IntakeConstants.kP).i(IntakeConstants.kI) .d(IntakeConstants.kD); m_PidConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - m_DeployMotor.configure(m_PidConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + m_deployMotor.configure(m_PidConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } - public void SpinIntake(AngularVelocity velocity) { - m_IntakeClosedLoopController.setSetpoint(velocity.in(RPM), ControlType.kVelocity); + public void spinIntake(AngularVelocity velocity) { + m_intakeClosedLoopController.setSetpoint(velocity.in(RPM), ControlType.kVelocity); } - public void StopIntake() { - SpinIntake(RPM.of(0)); + public void stopIntake() { + spinIntake(RPM.of(0)); } - public void DeployPosition() { + public void toDeployPosition() { + m_deployClosedLoopController.setSetpoint( + IntakeConstants.kDeployRotations.in(Rotations), + ControlType.kPosition); } -} - + public void retractIntake() { + m_deployClosedLoopController.setSetpoint( + IntakeConstants.kRetractRotations.in(Rotations), + ControlType.kPosition); + } +} From 64b1008e8cafb71be79bd181cfd99c111ac8b21f Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 7 Feb 2026 08:18:45 -0600 Subject: [PATCH 03/13] Set config for deploy motor --- src/main/java/frc/robot/Constants.java | 2 ++ .../frc/robot/subsystems/IntakeSubsystem.java | 16 ++++++++++------ 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9160a14..797be47 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -163,5 +163,7 @@ public static final class IntakeConstants { // Placeholder public static final Angle kDeployRotations = Rotations.of(10.0); public static final Angle kRetractRotations = Rotations.of(0.0); + + public static final int kDeployMotorCurrentLimit = 40; } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 9985c9e..ff64a21 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -11,6 +11,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.Constants.IntakeConstants; @@ -20,16 +21,19 @@ public class IntakeSubsystem { private final SparkMax m_deployMotor = new SparkMax(IntakeConstants.kMotorId, MotorType.kBrushless); private final SparkClosedLoopController m_intakeClosedLoopController = m_intakeMotor.getClosedLoopController(); - private final SparkClosedLoopController m_deployClosedLoopController = m_intakeMotor.getClosedLoopController(); + private final SparkClosedLoopController m_deployClosedLoopController = m_deployMotor.getClosedLoopController(); - private SparkMaxConfig m_PidConfig = new SparkMaxConfig(); + private SparkMaxConfig m_deployConfig = new SparkMaxConfig(); public IntakeSubsystem() { - m_PidConfig.closedLoop.p(IntakeConstants.kP).i(IntakeConstants.kI) + m_deployConfig.closedLoop.p(IntakeConstants.kP).i(IntakeConstants.kI) .d(IntakeConstants.kD); - m_PidConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - m_deployMotor.configure(m_PidConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + m_deployConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + m_deployConfig.idleMode(IdleMode.kBrake); + m_deployConfig.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); + m_deployMotor.configure(m_deployConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); } public void spinIntake(AngularVelocity velocity) { @@ -40,7 +44,7 @@ public void stopIntake() { spinIntake(RPM.of(0)); } - public void toDeployPosition() { + public void deployIntake() { m_deployClosedLoopController.setSetpoint( IntakeConstants.kDeployRotations.in(Rotations), ControlType.kPosition); From 997489efc03a795c70684ab808794fa8639d629d Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 7 Feb 2026 11:01:51 -0600 Subject: [PATCH 04/13] Add limit switches --- src/main/java/frc/robot/Constants.java | 7 ++++-- .../frc/robot/subsystems/IntakeSubsystem.java | 23 +++++++++++++++++-- 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 797be47..08a46fa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -160,10 +160,13 @@ public static final class IntakeConstants { public static final double kD = 0; public static final int kMotorId = 1; - // Placeholder - public static final Angle kDeployRotations = Rotations.of(10.0); + // 10 teeth on pinion, 20 teeth on rack + public static final Angle kDeployRotations = Rotations.of(20.0 / 10.0); public static final Angle kRetractRotations = Rotations.of(0.0); + public static final Angle kMaxExtension = Rotations.of(20.0 / 10.0); + public static final Angle kMinExtension = Rotations.of(0.0); + public static final int kDeployMotorCurrentLimit = 40; } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index ff64a21..098287a 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -4,32 +4,42 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkRelativeEncoder; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeConstants; +import com.revrobotics.spark.SparkLimitSwitch; -public class IntakeSubsystem { +public class IntakeSubsystem extends SubsystemBase { private final SparkMax m_intakeMotor = new SparkMax(IntakeConstants.kMotorId, MotorType.kBrushless); private final SparkMax m_deployMotor = new SparkMax(IntakeConstants.kMotorId, MotorType.kBrushless); private final SparkClosedLoopController m_intakeClosedLoopController = m_intakeMotor.getClosedLoopController(); private final SparkClosedLoopController m_deployClosedLoopController = m_deployMotor.getClosedLoopController(); + private final RelativeEncoder m_deployRelativeEncoder = m_deployMotor.getEncoder(); + + private final SparkLimitSwitch m_minLimitSwitch = m_intakeMotor.getReverseLimitSwitch(); + private final SparkLimitSwitch m_maxLimitSwitch = m_intakeMotor.getForwardLimitSwitch(); + private SparkMaxConfig m_deployConfig = new SparkMaxConfig(); public IntakeSubsystem() { m_deployConfig.closedLoop.p(IntakeConstants.kP).i(IntakeConstants.kI) .d(IntakeConstants.kD); - m_deployConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + // Apparently there is no absolute encoder :( + // m_deployConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); m_deployConfig.idleMode(IdleMode.kBrake); m_deployConfig.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); m_deployMotor.configure(m_deployConfig, ResetMode.kResetSafeParameters, @@ -55,4 +65,13 @@ public void retractIntake() { IntakeConstants.kRetractRotations.in(Rotations), ControlType.kPosition); } + + @Override + public void periodic() { + if (m_minLimitSwitch.isPressed()) { + m_deployRelativeEncoder.setPosition(IntakeConstants.kMinExtension.in(Rotations)); + } else if (m_maxLimitSwitch.isPressed()) { + m_deployRelativeEncoder.setPosition(IntakeConstants.kMaxExtension.in(Rotations)); + } + } } From 91519bf871f1fcccfb3a9c1e2501e477394ef28d Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Tue, 17 Feb 2026 17:43:19 -0600 Subject: [PATCH 05/13] test commands for intake --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 31 +++++++++++++++++++++ 2 files changed, 33 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dd6af1d..238b1d7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -269,5 +269,7 @@ public static final class IntakeConstants { public static final Angle kMinExtension = Rotations.of(0.0); public static final int kDeployMotorCurrentLimit = 40; + + public static final AngularVelocity kDefaultIntakeSpeed = RotationsPerSecond.of(1 * Math.PI); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 48d4fd4..1c24df4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,15 +14,20 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.OIConstants; import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.TurretSubsystem; public class RobotContainer { + private final IntakeSubsystem m_intake = new IntakeSubsystem(); + private final DriveSubsystem m_drive = new DriveSubsystem(); private final CommandXboxController m_driverController = new CommandXboxController( @@ -82,4 +87,30 @@ public Command feedPosition(Alliance alliance) { }, m_drive, m_turret); } + + public Command spinIntake() { + return new RunCommand(() -> { + m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed); + }); + } + + public Command retractIntake() { + return new RunCommand(() -> { + m_intake.stopIntake(); + m_intake.retractIntake(); + }); + } + + public Command outTake() { + return new RunCommand(() -> { + m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed.times(-1)); + }); + } + + public Command DeployIntake() { + return new InstantCommand(() -> { + m_intake.deployIntake(); + }); + + } } From 19671865e070884fbaaab6e90da5e4a3c6438824 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Mon, 23 Feb 2026 16:01:47 -0600 Subject: [PATCH 06/13] added second closed loop cotroller for the other motor so theres no potential offset --- .../frc/robot/subsystems/IntakeSubsystem.java | 42 +++++++++++++------ 1 file changed, 30 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 098287a..9aa1955 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -22,27 +22,39 @@ public class IntakeSubsystem extends SubsystemBase { private final SparkMax m_intakeMotor = new SparkMax(IntakeConstants.kMotorId, MotorType.kBrushless); - private final SparkMax m_deployMotor = new SparkMax(IntakeConstants.kMotorId, MotorType.kBrushless); + private final SparkMax m_deployMotor1 = new SparkMax(IntakeConstants.kMotorId, MotorType.kBrushless); + private final SparkMax m_deployMotor2 = new SparkMax(IntakeConstants.kMotorId, MotorType.kBrushless); private final SparkClosedLoopController m_intakeClosedLoopController = m_intakeMotor.getClosedLoopController(); - private final SparkClosedLoopController m_deployClosedLoopController = m_deployMotor.getClosedLoopController(); + private final SparkClosedLoopController m_deploy1ClosedLoopController = m_deployMotor1.getClosedLoopController(); + private final SparkClosedLoopController m_deploy2ClosedLoopController = m_deployMotor2.getClosedLoopController(); - private final RelativeEncoder m_deployRelativeEncoder = m_deployMotor.getEncoder(); + private final RelativeEncoder m_deploy1RelativeEncoder = m_deployMotor1.getEncoder(); + private final RelativeEncoder m_deploy2RelativeEncoder = m_deployMotor2.getEncoder(); private final SparkLimitSwitch m_minLimitSwitch = m_intakeMotor.getReverseLimitSwitch(); private final SparkLimitSwitch m_maxLimitSwitch = m_intakeMotor.getForwardLimitSwitch(); - private SparkMaxConfig m_deployConfig = new SparkMaxConfig(); + private SparkMaxConfig m_deploy1Config = new SparkMaxConfig(); + private SparkMaxConfig m_deploy2Config = new SparkMaxConfig(); public IntakeSubsystem() { - m_deployConfig.closedLoop.p(IntakeConstants.kP).i(IntakeConstants.kI) + m_deploy1Config.closedLoop.p(IntakeConstants.kP).i(IntakeConstants.kI) + .d(IntakeConstants.kD); + m_deploy2Config.closedLoop.p(IntakeConstants.kP).i(IntakeConstants.kI) .d(IntakeConstants.kD); // Apparently there is no absolute encoder :( // m_deployConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - m_deployConfig.idleMode(IdleMode.kBrake); - m_deployConfig.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); - m_deployMotor.configure(m_deployConfig, ResetMode.kResetSafeParameters, + m_deploy1Config.idleMode(IdleMode.kBrake); + m_deploy2Config.idleMode(IdleMode.kBrake); + + m_deploy1Config.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); + m_deploy2Config.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); + + m_deployMotor1.configure(m_deploy1Config, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + m_deployMotor2.configure(m_deploy1Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } @@ -55,13 +67,19 @@ public void stopIntake() { } public void deployIntake() { - m_deployClosedLoopController.setSetpoint( + m_deploy1ClosedLoopController.setSetpoint( + IntakeConstants.kDeployRotations.in(Rotations), + ControlType.kPosition); + m_deploy2ClosedLoopController.setSetpoint( IntakeConstants.kDeployRotations.in(Rotations), ControlType.kPosition); } public void retractIntake() { - m_deployClosedLoopController.setSetpoint( + m_deploy1ClosedLoopController.setSetpoint( + IntakeConstants.kRetractRotations.in(Rotations), + ControlType.kPosition); + m_deploy1ClosedLoopController.setSetpoint( IntakeConstants.kRetractRotations.in(Rotations), ControlType.kPosition); } @@ -69,9 +87,9 @@ public void retractIntake() { @Override public void periodic() { if (m_minLimitSwitch.isPressed()) { - m_deployRelativeEncoder.setPosition(IntakeConstants.kMinExtension.in(Rotations)); + m_deploy1RelativeEncoder.setPosition(IntakeConstants.kMinExtension.in(Rotations)); } else if (m_maxLimitSwitch.isPressed()) { - m_deployRelativeEncoder.setPosition(IntakeConstants.kMaxExtension.in(Rotations)); + m_deploy1RelativeEncoder.setPosition(IntakeConstants.kMaxExtension.in(Rotations)); } } } From a98e8325d4efc4e3e550f8f10d5af7e367c65059 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Tue, 24 Feb 2026 17:34:08 -0600 Subject: [PATCH 07/13] added the second relative encoder to the limit switch statement --- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 9aa1955..307bef3 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -88,8 +88,10 @@ public void retractIntake() { public void periodic() { if (m_minLimitSwitch.isPressed()) { m_deploy1RelativeEncoder.setPosition(IntakeConstants.kMinExtension.in(Rotations)); + m_deploy2RelativeEncoder.setPosition(IntakeConstants.kMinExtension.in(Rotations)); } else if (m_maxLimitSwitch.isPressed()) { m_deploy1RelativeEncoder.setPosition(IntakeConstants.kMaxExtension.in(Rotations)); + m_deploy2RelativeEncoder.setPosition(IntakeConstants.kMaxExtension.in(Rotations)); } } } From d2e877ab985fb60a274af0fb5a26e90b5c7871da Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Thu, 26 Feb 2026 16:51:04 -0600 Subject: [PATCH 08/13] added soft limits to deploy and retract functions --- .../frc/robot/subsystems/IntakeSubsystem.java | 26 ++++++++++++++----- 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 307bef3..026653e 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -67,21 +67,35 @@ public void stopIntake() { } public void deployIntake() { - m_deploy1ClosedLoopController.setSetpoint( + if (IntakeConstants.kMaxExtension.gt(Rotations.of(m_deploy1RelativeEncoder.getPosition())) + & IntakeConstants.kMaxExtension.gt(Rotations.of(m_deploy2RelativeEncoder.getPosition()))) { + m_deploy1ClosedLoopController.setSetpoint(IntakeConstants.kMaxExtension.in(Rotations), ControlType.kPosition); + m_deploy2ClosedLoopController.setSetpoint(IntakeConstants.kMaxExtension.in(Rotations), ControlType.kPosition); + System.out.println("Deploy position : " + IntakeConstants.kDeployRotations + "is to small"); + } else { + m_deploy1ClosedLoopController.setSetpoint( IntakeConstants.kDeployRotations.in(Rotations), ControlType.kPosition); - m_deploy2ClosedLoopController.setSetpoint( + m_deploy2ClosedLoopController.setSetpoint( IntakeConstants.kDeployRotations.in(Rotations), - ControlType.kPosition); + ControlType.kPosition); + } } public void retractIntake() { - m_deploy1ClosedLoopController.setSetpoint( + if (IntakeConstants.kMinExtension.lt(Rotations.of(m_deploy1RelativeEncoder.getPosition())) + & IntakeConstants.kMinExtension.lt(Rotations.of(m_deploy2RelativeEncoder.getPosition()))) { + m_deploy1ClosedLoopController.setSetpoint(IntakeConstants.kMinExtension.in(Rotations), ControlType.kPosition); + m_deploy2ClosedLoopController.setSetpoint(IntakeConstants.kMinExtension.in(Rotations), ControlType.kPosition); + System.out.println("Retract position : " + IntakeConstants.kRetractRotations + "is to small"); + } else{ + m_deploy1ClosedLoopController.setSetpoint( IntakeConstants.kRetractRotations.in(Rotations), ControlType.kPosition); - m_deploy1ClosedLoopController.setSetpoint( + m_deploy2ClosedLoopController.setSetpoint( IntakeConstants.kRetractRotations.in(Rotations), - ControlType.kPosition); + ControlType.kPosition); + } } @Override From e7acd4083280730d601f77c5cffc755942ce99e7 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Fri, 27 Feb 2026 16:46:26 -0600 Subject: [PATCH 09/13] Fixed greater than and less than statements --- .../frc/robot/subsystems/IntakeSubsystem.java | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 026653e..c439eaa 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -32,8 +32,10 @@ public class IntakeSubsystem extends SubsystemBase { private final RelativeEncoder m_deploy1RelativeEncoder = m_deployMotor1.getEncoder(); private final RelativeEncoder m_deploy2RelativeEncoder = m_deployMotor2.getEncoder(); - private final SparkLimitSwitch m_minLimitSwitch = m_intakeMotor.getReverseLimitSwitch(); - private final SparkLimitSwitch m_maxLimitSwitch = m_intakeMotor.getForwardLimitSwitch(); + private final SparkLimitSwitch m_minLimitSwitch1 = m_intakeMotor.getReverseLimitSwitch(); + private final SparkLimitSwitch m_minLimitSwitch2 = m_intakeMotor.getReverseLimitSwitch(); + private final SparkLimitSwitch m_maxLimitSwitch1 = m_intakeMotor.getForwardLimitSwitch(); + private final SparkLimitSwitch m_maxLimitSwitch2 = m_intakeMotor.getForwardLimitSwitch(); private SparkMaxConfig m_deploy1Config = new SparkMaxConfig(); private SparkMaxConfig m_deploy2Config = new SparkMaxConfig(); @@ -67,8 +69,8 @@ public void stopIntake() { } public void deployIntake() { - if (IntakeConstants.kMaxExtension.gt(Rotations.of(m_deploy1RelativeEncoder.getPosition())) - & IntakeConstants.kMaxExtension.gt(Rotations.of(m_deploy2RelativeEncoder.getPosition()))) { + if (IntakeConstants.kMaxExtension.lt(Rotations.of(m_deploy1RelativeEncoder.getPosition())) + & IntakeConstants.kMaxExtension.lt(Rotations.of(m_deploy2RelativeEncoder.getPosition()))) { m_deploy1ClosedLoopController.setSetpoint(IntakeConstants.kMaxExtension.in(Rotations), ControlType.kPosition); m_deploy2ClosedLoopController.setSetpoint(IntakeConstants.kMaxExtension.in(Rotations), ControlType.kPosition); System.out.println("Deploy position : " + IntakeConstants.kDeployRotations + "is to small"); @@ -83,8 +85,8 @@ public void deployIntake() { } public void retractIntake() { - if (IntakeConstants.kMinExtension.lt(Rotations.of(m_deploy1RelativeEncoder.getPosition())) - & IntakeConstants.kMinExtension.lt(Rotations.of(m_deploy2RelativeEncoder.getPosition()))) { + if (IntakeConstants.kMinExtension.gt(Rotations.of(m_deploy1RelativeEncoder.getPosition())) + & IntakeConstants.kMinExtension.gt(Rotations.of(m_deploy2RelativeEncoder.getPosition()))) { m_deploy1ClosedLoopController.setSetpoint(IntakeConstants.kMinExtension.in(Rotations), ControlType.kPosition); m_deploy2ClosedLoopController.setSetpoint(IntakeConstants.kMinExtension.in(Rotations), ControlType.kPosition); System.out.println("Retract position : " + IntakeConstants.kRetractRotations + "is to small"); @@ -100,10 +102,10 @@ public void retractIntake() { @Override public void periodic() { - if (m_minLimitSwitch.isPressed()) { + if (m_minLimitSwitch1.isPressed() || m_minLimitSwitch2.isPressed()) { m_deploy1RelativeEncoder.setPosition(IntakeConstants.kMinExtension.in(Rotations)); m_deploy2RelativeEncoder.setPosition(IntakeConstants.kMinExtension.in(Rotations)); - } else if (m_maxLimitSwitch.isPressed()) { + } else if (m_maxLimitSwitch1.isPressed() || m_maxLimitSwitch2.isPressed()) { m_deploy1RelativeEncoder.setPosition(IntakeConstants.kMaxExtension.in(Rotations)); m_deploy2RelativeEncoder.setPosition(IntakeConstants.kMaxExtension.in(Rotations)); } From 8358df2b6e47513f6dd49bbfa3c9333fca38e426 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 27 Feb 2026 17:28:05 -0600 Subject: [PATCH 10/13] Add angle --- .../frc/robot/subsystems/IntakeSubsystem.java | 59 +++++++++++-------- 1 file changed, 33 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index c439eaa..b8bf7ce 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -15,6 +15,7 @@ import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeConstants; @@ -50,10 +51,10 @@ public IntakeSubsystem() { // m_deployConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); m_deploy1Config.idleMode(IdleMode.kBrake); m_deploy2Config.idleMode(IdleMode.kBrake); - + m_deploy1Config.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); m_deploy2Config.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); - + m_deployMotor1.configure(m_deploy1Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); m_deployMotor2.configure(m_deploy1Config, ResetMode.kResetSafeParameters, @@ -69,34 +70,40 @@ public void stopIntake() { } public void deployIntake() { - if (IntakeConstants.kMaxExtension.lt(Rotations.of(m_deploy1RelativeEncoder.getPosition())) - & IntakeConstants.kMaxExtension.lt(Rotations.of(m_deploy2RelativeEncoder.getPosition()))) { - m_deploy1ClosedLoopController.setSetpoint(IntakeConstants.kMaxExtension.in(Rotations), ControlType.kPosition); - m_deploy2ClosedLoopController.setSetpoint(IntakeConstants.kMaxExtension.in(Rotations), ControlType.kPosition); - System.out.println("Deploy position : " + IntakeConstants.kDeployRotations + "is to small"); - } else { - m_deploy1ClosedLoopController.setSetpoint( - IntakeConstants.kDeployRotations.in(Rotations), - ControlType.kPosition); - m_deploy2ClosedLoopController.setSetpoint( - IntakeConstants.kDeployRotations.in(Rotations), - ControlType.kPosition); + Angle deploy1Position = Rotations.of(m_deploy1RelativeEncoder.getPosition()); + Angle deploy2Position = Rotations.of(m_deploy2RelativeEncoder.getPosition()); + + if (deploy1Position.lt(IntakeConstants.kMaxExtension)) { + m_deploy1ClosedLoopController.setSetpoint(IntakeConstants.kDeployRotations.in(Rotations), + ControlType.kPosition); + } else { + System.out.println("Attempting to extend when beyond limit hitting limit on deploy motor 1."); + } + + if (deploy2Position.lt(IntakeConstants.kMaxExtension)) { + m_deploy1ClosedLoopController.setSetpoint(IntakeConstants.kDeployRotations.in(Rotations), + ControlType.kPosition); + } else { + System.out.println("Attempting to extend beyond limit when hitting limit on deploy motor 2."); } } public void retractIntake() { - if (IntakeConstants.kMinExtension.gt(Rotations.of(m_deploy1RelativeEncoder.getPosition())) - & IntakeConstants.kMinExtension.gt(Rotations.of(m_deploy2RelativeEncoder.getPosition()))) { - m_deploy1ClosedLoopController.setSetpoint(IntakeConstants.kMinExtension.in(Rotations), ControlType.kPosition); - m_deploy2ClosedLoopController.setSetpoint(IntakeConstants.kMinExtension.in(Rotations), ControlType.kPosition); - System.out.println("Retract position : " + IntakeConstants.kRetractRotations + "is to small"); - } else{ - m_deploy1ClosedLoopController.setSetpoint( - IntakeConstants.kRetractRotations.in(Rotations), - ControlType.kPosition); - m_deploy2ClosedLoopController.setSetpoint( - IntakeConstants.kRetractRotations.in(Rotations), - ControlType.kPosition); + Angle deploy1Position = Rotations.of(m_deploy1RelativeEncoder.getPosition()); + Angle deploy2Position = Rotations.of(m_deploy2RelativeEncoder.getPosition()); + + if (deploy1Position.gt(IntakeConstants.kMinExtension)) { + m_deploy1ClosedLoopController.setSetpoint(IntakeConstants.kDeployRotations.in(Rotations), + ControlType.kPosition); + } else { + System.out.println("Attempting to retract beyond limit when hitting limit on deploy motor 1."); + } + + if (deploy2Position.gt(IntakeConstants.kMinExtension)) { + m_deploy1ClosedLoopController.setSetpoint(IntakeConstants.kDeployRotations.in(Rotations), + ControlType.kPosition); + } else { + System.out.println("Attempting to retract beyond limit when hitting limit on deploy motor 2."); } } From 5e3229fe09e380f6cd6682309a0dcf8d5e9c6bbf Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 28 Feb 2026 14:17:46 -0600 Subject: [PATCH 11/13] Working intake PID numbers --- src/main/java/frc/robot/Constants.java | 11 ++++++----- src/main/java/frc/robot/RobotContainer.java | 14 ++++++++++++-- .../frc/robot/subsystems/IntakeSubsystem.java | 13 +++++++------ .../frc/robot/subsystems/TurretSubsystem.java | 19 ++++++++++--------- 4 files changed, 35 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ed23662..377171e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -348,7 +348,7 @@ public static final class StagingConstants { public static double kFeedIntoHoodSpeed = 0.10; public static final int kAgitationMotorId = 9; - public static final double kAgitationSpeed = 0.00; + public static final double kAgitationSpeed = -0.15; public static final int kRollerMotorId = 12; public static final double kRollerSpeed = 0.25; @@ -406,9 +406,10 @@ public static final class IntakeConstants { public static final double kI2 = 0; public static final double kD2 = 0; - public static final double kPIn = 0.1; + public static final double kPIn = 0.0003; public static final double kIIn = 0; - public static final double kDIn = 0; + public static final double kDIn = 0.0005; + public static final double kFFIn = 0.00192; public static final int kDeployMotor1Id = 13; public static final int kDeployMotor2Id = 8; @@ -422,8 +423,8 @@ public static final class IntakeConstants { public static final Angle kMinExtension = Rotations.of(0.0); public static final int kDeployMotorCurrentLimit = 40; - public static final int kIntakeMotorCurrentLimit = 30; + public static final int kIntakeMotorCurrentLimit = 80; - public static final AngularVelocity kDefaultIntakeSpeed = RotationsPerSecond.of(-(0.01 * Math.PI)); + public static final AngularVelocity kDefaultIntakeSpeed = RPM.of(-2000); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 11ac375..b66d95b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -101,13 +101,22 @@ private void configureBindings() { // System.out.println("Bindings configured"); // m_driverController.x().whileTrue(m_aimFactory.PointAtHub(true)); - m_driverController.x().whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); + // m_driverController.x().whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); // m_driverController.x().whileTrue( // m_aimFactory.Shoot(ShooterConstants.kFeedingWheelVelocity) // .finallyDo(() -> m_aimFactory.Shoot(RPM.of(0.0)))); // m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); + + m_driverController.x().onTrue(DeployIntake()); + + m_driverController.a().onTrue(retractIntake()); + + m_driverController.b().whileTrue(spinIntake()); + + m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); + } public Command getAutonomousCommand() { @@ -133,7 +142,7 @@ public Command feedPosition(Alliance alliance) { public Command spinIntake() { return new RunCommand(() -> { m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed); - }); + }).finallyDo(m_intake::stopIntake); } public Command retractIntake() { @@ -143,6 +152,7 @@ public Command retractIntake() { }); } + public Command outTake() { return new RunCommand(() -> { m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed.times(-1)); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 7ca1031..ac76a3a 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -55,13 +55,14 @@ public IntakeSubsystem() { .d(IntakeConstants.kD1); m_deploy2Config.closedLoop.p(IntakeConstants.kP2).i(IntakeConstants.kI2) .d(IntakeConstants.kD2); - m_intakeConfig.closedLoop.p(IntakeConstants.kPIn).i(IntakeConstants.kIIn).d(IntakeConstants.kDIn); + m_intakeConfig.closedLoop.p(IntakeConstants.kPIn).i(IntakeConstants.kIIn).d(IntakeConstants.kDIn).feedForward + .kV(IntakeConstants.kFFIn); // Apparently there is no absolute encoder :( // m_deployConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); m_deploy1Config.idleMode(IdleMode.kCoast); m_deploy2Config.idleMode(IdleMode.kCoast); // TODO: change to brake m_intakeConfig.idleMode(IdleMode.kBrake); - + m_deploy1Config.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); m_deploy2Config.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); m_intakeConfig.smartCurrentLimit(IntakeConstants.kIntakeMotorCurrentLimit); @@ -71,8 +72,8 @@ public IntakeSubsystem() { PersistMode.kPersistParameters); m_deployMotor2.configure(m_deploy2Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - m_intakeMotor.configure(m_intakeConfig, ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters); + m_intakeMotor.configure(m_intakeConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); } public void spinIntake(AngularVelocity velocity) { @@ -113,14 +114,14 @@ public void retractIntake() { if (deploy1Position.gt(IntakeConstants.kMinExtension)) { m_deploy1ClosedLoopController.setSetpoint(IntakeConstants.kRetractRotations.in(Rotations), - ControlType.kPosition); + ControlType.kPosition); } else { System.out.println("Attempting to retract beyond limit when hitting limit on deploy motor 1."); } if (deploy2Position.gt(IntakeConstants.kMinExtension)) { m_deploy2ClosedLoopController.setSetpoint(IntakeConstants.kRetractRotations.in(Rotations), - ControlType.kPosition); + ControlType.kPosition); } else { System.out.println("Attempting to retract beyond limit when hitting limit on deploy motor 2."); } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 5e40b52..e7d8101 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -56,6 +56,7 @@ public class TurretSubsystem extends SubsystemBase { private MechanismLigament2d m_max1 = new MechanismLigament2d("max1", 2, 0); private MechanismLigament2d m_min2 = new MechanismLigament2d("min2", 2, 0); private MechanismLigament2d m_max2 = new MechanismLigament2d("max2", 2, 0); + private MechanismLigament2d m_robotHeading = new MechanismLigament2d("robotHeading", 2, 0); private Angle robotRotation; @@ -90,6 +91,9 @@ public TurretSubsystem() { m_max2 = m_mechRoot.append(m_max2); m_max2.setColor(new Color8Bit("#FF0000")); + m_robotHeading = m_mechRoot.append(m_robotHeading); + m_robotHeading.setColor(new Color8Bit("#32CD32")); + robotRotation = Degrees.of(0); } @@ -122,7 +126,7 @@ public Angle getRotation() { Rotations.of(m_absoluteEncoder.getPosition()) .minus(TurretConstants.kAngularDistanceToFrontOfRobot)); - return m_targetAngle; + return UtilityFunctions.WrapAngle(m_targetAngle.minus(TurretConstants.kAngularDistanceToFrontOfRobot)); } public void addDriveHeading(Angle angle) { @@ -162,14 +166,11 @@ public LinearVelocity getMuzzleVelocityAtHoodAngle() { public void simulationPeriodic() { m_simLigament.setAngle( m_targetAngle.plus(robotRotation).minus(TurretConstants.kAngularDistanceToFrontOfRobot).in(Degrees)); - m_min1.setAngle(TurretConstants.kHubMinAngle1.plus(robotRotation) - .minus(TurretConstants.kAngularDistanceToFrontOfRobot).in(Degrees)); - m_max1.setAngle(TurretConstants.kHubMaxAngle1.plus(robotRotation) - .minus(TurretConstants.kAngularDistanceToFrontOfRobot).in(Degrees)); - m_min2.setAngle(TurretConstants.kHubMinAngle2.plus(robotRotation) - .minus(TurretConstants.kAngularDistanceToFrontOfRobot).in(Degrees)); - m_max2.setAngle(TurretConstants.kHubMaxAngle2.plus(robotRotation) - .minus(TurretConstants.kAngularDistanceToFrontOfRobot).in(Degrees)); + m_min1.setAngle(TurretConstants.kHubMinAngle1.plus(robotRotation).in(Degrees)); + m_max1.setAngle(TurretConstants.kHubMaxAngle1.plus(robotRotation).in(Degrees)); + m_min2.setAngle(TurretConstants.kHubMinAngle2.plus(robotRotation).in(Degrees)); + m_max2.setAngle(TurretConstants.kHubMaxAngle2.plus(robotRotation).in(Degrees)); + m_robotHeading.setAngle(robotRotation.in(Degrees)); SmartDashboard.putData("Turret Rotation", m_simMech); } From 0e67f2e8929e3e2eb6ccbb90fe6840cc490b95af Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 28 Feb 2026 14:29:48 -0600 Subject: [PATCH 12/13] commented out test button bindings --- src/main/java/frc/robot/RobotContainer.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b66d95b..a9b2876 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -109,13 +109,13 @@ private void configureBindings() { // m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); - m_driverController.x().onTrue(DeployIntake()); + // m_driverController.x().onTrue(DeployIntake()); - m_driverController.a().onTrue(retractIntake()); + // m_driverController.a().onTrue(retractIntake()); - m_driverController.b().whileTrue(spinIntake()); + // m_driverController.b().whileTrue(spinIntake()); - m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); + // m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); } @@ -152,7 +152,6 @@ public Command retractIntake() { }); } - public Command outTake() { return new RunCommand(() -> { m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed.times(-1)); From cebb1b76df6e6e15408337b30eab76c7df357bcb Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 28 Feb 2026 15:42:14 -0600 Subject: [PATCH 13/13] Fix intake command scheduling problem --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 11 +++++------ .../java/frc/robot/subsystems/IntakeSubsystem.java | 8 ++++---- 3 files changed, 11 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 377171e..0cc2936 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -416,10 +416,10 @@ public static final class IntakeConstants { public static final int kIntakeMotorId = 7; // 10 teeth on pinion, 20 teeth on rack - public static final Angle kDeployRotations = Rotations.of(20.0 / 10.0).times(5.0); + public static final Angle kDeployRotations = Rotations.of(9.5); public static final Angle kRetractRotations = Rotations.of(0.0); - public static final Angle kMaxExtension = Rotations.of(20.0 / 10.0).times(5.0); + public static final Angle kMaxExtension = Rotations.of(9.5); public static final Angle kMinExtension = Rotations.of(0.0); public static final int kDeployMotorCurrentLimit = 40; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a9b2876..6e0b03c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -109,13 +109,13 @@ private void configureBindings() { // m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); - // m_driverController.x().onTrue(DeployIntake()); + m_driverController.x().onTrue(DeployIntake()); - // m_driverController.a().onTrue(retractIntake()); + m_driverController.a().onTrue(retractIntake()); - // m_driverController.b().whileTrue(spinIntake()); + m_driverController.b().whileTrue(spinIntake()); - // m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); + m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); } @@ -146,8 +146,7 @@ public Command spinIntake() { } public Command retractIntake() { - return new RunCommand(() -> { - m_intake.stopIntake(); + return new InstantCommand(() -> { m_intake.retractIntake(); }); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index ac76a3a..ec9385c 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -165,10 +165,10 @@ public void periodic() { DogLog.log("Deploy motor can ID 8 setpoint", m_deploy2ClosedLoopController.getSetpoint()); DogLog.log("Deploy motor can ID 13 setpoint", m_deploy1ClosedLoopController.getSetpoint()); - // DogLog.log("Min limit one", m_minLimitSwitch1.isPressed()); - // DogLog.log("Min limit two", m_minLimitSwitch2.isPressed()); + DogLog.log("Min limit one", m_minLimitSwitch1.isPressed()); + DogLog.log("Min limit two", m_minLimitSwitch2.isPressed()); - // DogLog.log("Max limit one", m_maxLimitSwitch1.isPressed()); - // DogLog.log("Max limit two", m_maxLimitSwitch2.isPressed()); + DogLog.log("Max limit one", m_maxLimitSwitch1.isPressed()); + DogLog.log("Max limit two", m_maxLimitSwitch2.isPressed()); } }