From 68c02899c03897ac0676b9f071233b4f94349c80 Mon Sep 17 00:00:00 2001 From: Parneel Bhakhri Date: Wed, 8 Oct 2025 19:12:41 -0700 Subject: [PATCH 1/5] resolve errors --- build.gradle | 2 +- .../frc/robot/AutoRoutines/AutoProducer.java | 6 +- src/main/java/frc/robot/Robot.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 6 +- .../frc/robot/commandGroups/D2Intake.java | 4 +- .../frc/robot/commandGroups/Dealgaenate.java | 4 +- .../frc/robot/commandGroups/EjectCoralFR.java | 4 +- .../frc/robot/commandGroups/ElevatorL4.java | 4 +- .../java/frc/robot/commandGroups/Intake.java | 4 +- .../robot/commandGroups/JamesHardenScore.java | 6 +- .../robot/commandGroups/PutUpAndShoot.java | 4 +- .../RunFunnelUntilDetectionSafeSmooth.java | 4 +- .../java/frc/robot/commandGroups/ShootL1.java | 5 +- .../robot/commandGroups/ShootL1Funnel.java | 4 +- .../commandGroups/UnjamFunnelAndIntake.java | 4 +- .../ArmToAngleAndSpinFlywheel.java | 8 +- .../ElevatorCommands/DefaultElevator.java | 8 +- .../ElevatorCommands/ElevatorHoldL4.java | 10 +- .../ElevatorCommands/SetElevatorLevel.java | 16 +- .../SetElevatorLevelInstant.java | 8 +- .../ZeroElevatorHardStop.java | 9 +- .../ZeroElevatorToFFiltered.java | 10 +- .../commands/FunnelCommands/RampUpFunnel.java | 8 +- .../RunFunnelUntilDetectionSafe.java | 9 +- ...TransferPieceBetweenFunnelAndElevator.java | 8 +- .../subsystems/ElevatorSubsystemMD2.java | 152 ++++++++++++++++++ .../frc/robot/subsystems/VisionSystem.java | 2 +- 27 files changed, 235 insertions(+), 80 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java diff --git a/build.gradle b/build.gradle index 909d64a9..3be07aa8 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.2.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" id 'com.diffplug.spotless' version '6.20.0' } diff --git a/src/main/java/frc/robot/AutoRoutines/AutoProducer.java b/src/main/java/frc/robot/AutoRoutines/AutoProducer.java index 9f1b4256..eccd0d6a 100644 --- a/src/main/java/frc/robot/AutoRoutines/AutoProducer.java +++ b/src/main/java/frc/robot/AutoRoutines/AutoProducer.java @@ -23,7 +23,7 @@ import frc.robot.commands.TootsieSlideCommands.ShootTootsieSlide; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.CoralPosition; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.LedSubsystem; import frc.robot.subsystems.SwerveSubsystem; @@ -36,7 +36,7 @@ public class AutoProducer extends SequentialCommandGroup { public AutoProducer( SwerveSubsystem driveTrain, TootsieSlideSubsystem shooter, - ElevatorSubsystem elevator, + ElevatorSubsystemMD2 elevator, FunnelSubsystem funnel, ArmSubsystem arm, List autoInformation, @@ -83,7 +83,7 @@ public AutoProducer( } private void settyCycle( - ElevatorSubsystem elevator, + ElevatorSubsystemMD2 elevator, FunnelSubsystem funnel, TootsieSlideSubsystem shooter, SwerveSubsystem driveTrain, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1743b9ec..a41de565 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.commands.ElevatorCommands.ZeroElevatorHardStop; import frc.robot.subsystems.CoralPosition; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.SwerveSubsystem; import frc.robot.subsystems.VisionSystem; import frc.robot.util.LoggedTalonFX; @@ -144,9 +144,9 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - if (ElevatorSubsystem.getInstance().isElevatorZeroed() == false) { + if (ElevatorSubsystemMD2.getInstance().isElevatorZeroed() == false) { CommandScheduler.getInstance() - .schedule(new ZeroElevatorHardStop(ElevatorSubsystem.getInstance())); + .schedule(new ZeroElevatorHardStop(ElevatorSubsystemMD2.getInstance())); } // CommandScheduler.getInstance(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 404e2792..b5e9a6b9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -38,7 +38,7 @@ import frc.robot.commands.TransferPieceBetweenFunnelAndElevator; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.CoralPosition; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.LedSubsystem; import frc.robot.subsystems.SwerveSubsystem; @@ -53,7 +53,7 @@ public class RobotContainer { TootsieSlideSubsystem tootsieSlideSubsystem = TootsieSlideSubsystem.getInstance(); FunnelSubsystem funnelSubsystem = FunnelSubsystem.getInstance(); - ElevatorSubsystem elevatorSubsystem = ElevatorSubsystem.getInstance(); + ElevatorSubsystemMD2 elevatorSubsystem = ElevatorSubsystemMD2.getInstance(); ArmSubsystem armSubsystem = ArmSubsystem.getInstance(); LedSubsystem leds = new LedSubsystem(); // Alliance color @@ -286,7 +286,7 @@ private void configureBindings() { () -> CoralPosition.isCoralInFunnel() && elevatorSubsystem.atIntake() - && elevatorSubsystem.isAtPosition()) + && elevatorSubsystem.isAtTargetHeight()) .and(RobotModeTriggers.teleop()); funnelCheckout diff --git a/src/main/java/frc/robot/commandGroups/D2Intake.java b/src/main/java/frc/robot/commandGroups/D2Intake.java index 765482d5..412188f7 100644 --- a/src/main/java/frc/robot/commandGroups/D2Intake.java +++ b/src/main/java/frc/robot/commandGroups/D2Intake.java @@ -5,13 +5,13 @@ import frc.robot.commands.ElevatorCommands.SetElevatorLevel; import frc.robot.commands.TootsieSlideCommands.ShootTootsieSlide; import frc.robot.commands.TransferPieceBetweenFunnelAndElevator; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.TootsieSlideSubsystem; public class D2Intake extends SequentialCommandGroup { public D2Intake( - ElevatorSubsystem elevatorSubsystem, + ElevatorSubsystemMD2 elevatorSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem, FunnelSubsystem funnelSubsystem) { addCommands( diff --git a/src/main/java/frc/robot/commandGroups/Dealgaenate.java b/src/main/java/frc/robot/commandGroups/Dealgaenate.java index e8b16180..84a3d1e0 100644 --- a/src/main/java/frc/robot/commandGroups/Dealgaenate.java +++ b/src/main/java/frc/robot/commandGroups/Dealgaenate.java @@ -10,14 +10,14 @@ import frc.robot.commands.DaleCommands.ArmToAngleAndSpinFlywheel; import frc.robot.commands.ElevatorCommands.SetElevatorLevel; import frc.robot.subsystems.ArmSubsystem; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class Dealgaenate extends SequentialCommandGroup { /** Creates a new Dealgaenate. */ - public Dealgaenate(ArmSubsystem arm, ElevatorSubsystem elevator, ElevatorPositions position) { + public Dealgaenate(ArmSubsystem arm, ElevatorSubsystemMD2 elevator, ElevatorPositions position) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); diff --git a/src/main/java/frc/robot/commandGroups/EjectCoralFR.java b/src/main/java/frc/robot/commandGroups/EjectCoralFR.java index 20f3221f..da237e90 100644 --- a/src/main/java/frc/robot/commandGroups/EjectCoralFR.java +++ b/src/main/java/frc/robot/commandGroups/EjectCoralFR.java @@ -4,12 +4,12 @@ import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.commands.ElevatorCommands.SetElevatorLevel; import frc.robot.commands.TootsieSlideCommands.ShootTootsieSlide; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.TootsieSlideSubsystem; public class EjectCoralFR extends SequentialCommandGroup { public EjectCoralFR( - ElevatorSubsystem elevatorSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem) { + ElevatorSubsystemMD2 elevatorSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem) { addCommands( new SetElevatorLevel(elevatorSubsystem, ElevatorPositions.L1, false) .andThen(new ShootTootsieSlide(tootsieSlideSubsystem).withTimeout(0.5))); diff --git a/src/main/java/frc/robot/commandGroups/ElevatorL4.java b/src/main/java/frc/robot/commandGroups/ElevatorL4.java index 2c1960ad..46eac388 100644 --- a/src/main/java/frc/robot/commandGroups/ElevatorL4.java +++ b/src/main/java/frc/robot/commandGroups/ElevatorL4.java @@ -4,11 +4,11 @@ import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.commands.ElevatorCommands.ElevatorHoldL4; import frc.robot.commands.ElevatorCommands.SetElevatorLevel; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; public class ElevatorL4 extends SequentialCommandGroup { - public ElevatorL4(ElevatorSubsystem elevatorSubsystem, boolean checkIfCoralInTootsie) { + public ElevatorL4(ElevatorSubsystemMD2 elevatorSubsystem, boolean checkIfCoralInTootsie) { addCommands( new SetElevatorLevel(elevatorSubsystem, ElevatorPositions.L4, checkIfCoralInTootsie) diff --git a/src/main/java/frc/robot/commandGroups/Intake.java b/src/main/java/frc/robot/commandGroups/Intake.java index c2c3b924..a661204e 100644 --- a/src/main/java/frc/robot/commandGroups/Intake.java +++ b/src/main/java/frc/robot/commandGroups/Intake.java @@ -8,7 +8,7 @@ import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.commands.ElevatorCommands.SetElevatorLevel; import frc.robot.commands.TransferPieceBetweenFunnelAndElevator; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.LedSubsystem; import frc.robot.subsystems.TootsieSlideSubsystem; @@ -19,7 +19,7 @@ public class Intake extends SequentialCommandGroup { public Intake( - ElevatorSubsystem elevatorSubsystem, + ElevatorSubsystemMD2 elevatorSubsystem, FunnelSubsystem funnelSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem, LedSubsystem leds) { diff --git a/src/main/java/frc/robot/commandGroups/JamesHardenScore.java b/src/main/java/frc/robot/commandGroups/JamesHardenScore.java index 37e05084..280459bd 100644 --- a/src/main/java/frc/robot/commandGroups/JamesHardenScore.java +++ b/src/main/java/frc/robot/commandGroups/JamesHardenScore.java @@ -10,7 +10,7 @@ import frc.robot.commands.EndWhenCloseEnough; import frc.robot.commands.SwerveCommands.JamesHardenMovement; import frc.robot.commands.TootsieSlideCommands.ShootTootsieSlide; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.LedSubsystem; import frc.robot.subsystems.SwerveSubsystem; import frc.robot.subsystems.TootsieSlideSubsystem; @@ -18,7 +18,7 @@ public class JamesHardenScore extends SequentialCommandGroup { public JamesHardenScore( - ElevatorSubsystem elevatorSubsystem, + ElevatorSubsystemMD2 elevatorSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem, SwerveSubsystem swerveSubsystem, ElevatorPositions height, @@ -64,7 +64,7 @@ public JamesHardenScore( } public JamesHardenScore( - ElevatorSubsystem elevatorSubsystem, + ElevatorSubsystemMD2 elevatorSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem, SwerveSubsystem swerveSubsystem, ElevatorPositions height, diff --git a/src/main/java/frc/robot/commandGroups/PutUpAndShoot.java b/src/main/java/frc/robot/commandGroups/PutUpAndShoot.java index 3ec1f872..3f5070fc 100644 --- a/src/main/java/frc/robot/commandGroups/PutUpAndShoot.java +++ b/src/main/java/frc/robot/commandGroups/PutUpAndShoot.java @@ -4,12 +4,12 @@ import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.commands.ElevatorCommands.SetElevatorLevel; import frc.robot.commands.TootsieSlideCommands.ShootTootsieSlide; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.TootsieSlideSubsystem; public class PutUpAndShoot extends SequentialCommandGroup { public PutUpAndShoot( - ElevatorSubsystem elevatorSubsystem, + ElevatorSubsystemMD2 elevatorSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem, ElevatorPositions height) { diff --git a/src/main/java/frc/robot/commandGroups/RunFunnelUntilDetectionSafeSmooth.java b/src/main/java/frc/robot/commandGroups/RunFunnelUntilDetectionSafeSmooth.java index 807c4814..203e3b4e 100644 --- a/src/main/java/frc/robot/commandGroups/RunFunnelUntilDetectionSafeSmooth.java +++ b/src/main/java/frc/robot/commandGroups/RunFunnelUntilDetectionSafeSmooth.java @@ -4,14 +4,14 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.FunnelCommands.RampUpFunnel; import frc.robot.commands.FunnelCommands.RunFunnelUntilDetectionSafe; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.LedSubsystem; import java.util.function.BooleanSupplier; public class RunFunnelUntilDetectionSafeSmooth extends SequentialCommandGroup { public RunFunnelUntilDetectionSafeSmooth( - ElevatorSubsystem elevatorSubsystem, FunnelSubsystem funnelSubsystem, LedSubsystem leds) { + ElevatorSubsystemMD2 elevatorSubsystem, FunnelSubsystem funnelSubsystem, LedSubsystem leds) { addCommands( // new WaitCommand(0.1), diff --git a/src/main/java/frc/robot/commandGroups/ShootL1.java b/src/main/java/frc/robot/commandGroups/ShootL1.java index b6780681..de94c0c9 100644 --- a/src/main/java/frc/robot/commandGroups/ShootL1.java +++ b/src/main/java/frc/robot/commandGroups/ShootL1.java @@ -4,11 +4,12 @@ import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.commands.ElevatorCommands.SetElevatorLevel; import frc.robot.commands.TootsieSlideCommands.ShootSlow; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.TootsieSlideSubsystem; public class ShootL1 extends SequentialCommandGroup { - public ShootL1(ElevatorSubsystem elevatorSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem) { + public ShootL1( + ElevatorSubsystemMD2 elevatorSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem) { addCommands( new SetElevatorLevel(elevatorSubsystem, ElevatorPositions.L1, false) .andThen(new ShootSlow(tootsieSlideSubsystem)) diff --git a/src/main/java/frc/robot/commandGroups/ShootL1Funnel.java b/src/main/java/frc/robot/commandGroups/ShootL1Funnel.java index 8e3460c1..67c69edb 100644 --- a/src/main/java/frc/robot/commandGroups/ShootL1Funnel.java +++ b/src/main/java/frc/robot/commandGroups/ShootL1Funnel.java @@ -6,13 +6,13 @@ import frc.robot.commands.ElevatorCommands.SetElevatorLevel; import frc.robot.commands.FunnelCommands.ReverseFunnel; import frc.robot.commands.TootsieSlideCommands.ReverseTootsie; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.TootsieSlideSubsystem; public class ShootL1Funnel extends SequentialCommandGroup { public ShootL1Funnel( - ElevatorSubsystem elevatorSubsystem, + ElevatorSubsystemMD2 elevatorSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem, FunnelSubsystem funnelSubsystem) { // if (CoralPosition.isCoralInTootsieSlide()) { diff --git a/src/main/java/frc/robot/commandGroups/UnjamFunnelAndIntake.java b/src/main/java/frc/robot/commandGroups/UnjamFunnelAndIntake.java index 76dcadb7..e5a528a1 100644 --- a/src/main/java/frc/robot/commandGroups/UnjamFunnelAndIntake.java +++ b/src/main/java/frc/robot/commandGroups/UnjamFunnelAndIntake.java @@ -5,13 +5,13 @@ import frc.robot.commands.ElevatorCommands.SetElevatorLevel; import frc.robot.commands.FunnelCommands.RunFunnelAndTootsieInCommand; import frc.robot.commands.FunnelCommands.RunFunnelOutUntilUnstuckCommand; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.TootsieSlideSubsystem; public class UnjamFunnelAndIntake extends SequentialCommandGroup { public UnjamFunnelAndIntake( - ElevatorSubsystem elevatorSubsystem, + ElevatorSubsystemMD2 elevatorSubsystem, FunnelSubsystem funnelSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem) { addCommands( diff --git a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java index a5d410af..33ae063d 100644 --- a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java +++ b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java @@ -5,16 +5,16 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.subsystems.ArmSubsystem; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; public class ArmToAngleAndSpinFlywheel extends Command { private final ArmSubsystem armPlusFlywheel; private double angle; private double tolerance = 5; - private ElevatorSubsystem elevatorSubsystem; + private ElevatorSubsystemMD2 elevatorSubsystem; public ArmToAngleAndSpinFlywheel( - double angle, ArmSubsystem armSub, ElevatorSubsystem elevatorSubsystem) { + double angle, ArmSubsystem armSub, ElevatorSubsystemMD2 elevatorSubsystem) { armPlusFlywheel = armSub; this.angle = angle; this.elevatorSubsystem = elevatorSubsystem; @@ -34,7 +34,7 @@ public void execute() { @Override public void end(boolean interrupted) { armPlusFlywheel.stopFlywheel(); // Stop the flywheel - elevatorSubsystem.elevateTo(ElevatorPositions.safePosition); + elevatorSubsystem.setHeight(ElevatorPositions.safePosition.height); } @Override diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/DefaultElevator.java b/src/main/java/frc/robot/commands/ElevatorCommands/DefaultElevator.java index 73e1b905..4305ce85 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/DefaultElevator.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/DefaultElevator.java @@ -3,15 +3,15 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.subsystems.CoralPosition; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; public class DefaultElevator extends Command { - private final ElevatorSubsystem elevatorSubsystem; + private final ElevatorSubsystemMD2 elevatorSubsystem; // private WindowAverage windowAverage = new WindowAverage(); // private double currentAverage = 0.0; - public DefaultElevator(ElevatorSubsystem subsystem) { + public DefaultElevator(ElevatorSubsystemMD2 subsystem) { elevatorSubsystem = subsystem; addRequirements(elevatorSubsystem); } @@ -41,7 +41,7 @@ public void execute() { // } if (!CoralPosition.isCoralInTootsieSlide()) { - elevatorSubsystem.elevateTo(ElevatorPositions.Intake); + elevatorSubsystem.setHeight(ElevatorPositions.Intake.height); } } diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorHoldL4.java b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorHoldL4.java index f61019cc..d6df823b 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorHoldL4.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorHoldL4.java @@ -2,12 +2,12 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ElevatorConstants.ElevatorPositions; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; public class ElevatorHoldL4 extends Command { - private ElevatorSubsystem elevatorSubsystem; + private ElevatorSubsystemMD2 elevatorSubsystem; - public ElevatorHoldL4(ElevatorSubsystem elevatorSubsystem) { + public ElevatorHoldL4(ElevatorSubsystemMD2 elevatorSubsystem) { this.elevatorSubsystem = elevatorSubsystem; addRequirements(elevatorSubsystem); } @@ -15,7 +15,7 @@ public ElevatorHoldL4(ElevatorSubsystem elevatorSubsystem) { // Called when the command is initially scheduled. @Override public void initialize() { - elevatorSubsystem.elevateTo(ElevatorPositions.LIMIT_OF_TRAVEL); + elevatorSubsystem.setHeight(ElevatorPositions.LIMIT_OF_TRAVEL.height); } // Called every time the scheduler runs while the command is scheduled. @@ -31,6 +31,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return elevatorSubsystem.isAtPosition(); + return elevatorSubsystem.isAtTargetHeight(); } } diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevel.java b/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevel.java index 88a4e8de..169c8c86 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevel.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevel.java @@ -3,16 +3,18 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.subsystems.CoralPosition; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; public class SetElevatorLevel extends Command { - private final ElevatorSubsystem elevatorSubsystem; + private final ElevatorSubsystemMD2 elevatorSubsystem; private final ElevatorPositions pos; private final boolean checkIfCoralInTootsie; public SetElevatorLevel( - ElevatorSubsystem subsystem, ElevatorPositions pos, boolean checkIfCoralInTootsie) { - elevatorSubsystem = subsystem; + ElevatorSubsystemMD2 elevatorSubsystem2, + ElevatorPositions pos, + boolean checkIfCoralInTootsie) { + elevatorSubsystem = elevatorSubsystem2; this.pos = pos; this.checkIfCoralInTootsie = checkIfCoralInTootsie; addRequirements(elevatorSubsystem); @@ -25,10 +27,10 @@ public void initialize() {} public void execute() { if (checkIfCoralInTootsie) { if (CoralPosition.isCoralInTootsieSlide()) { - elevatorSubsystem.elevateTo(pos); + elevatorSubsystem.setHeight(pos.height); } } else { - elevatorSubsystem.elevateTo(pos); + elevatorSubsystem.setHeight(pos.height); } } @@ -37,6 +39,6 @@ public void end(boolean interrupted) {} @Override public boolean isFinished() { - return elevatorSubsystem.isAtPosition(); + return elevatorSubsystem.isAtTargetHeight(); } } diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevelInstant.java b/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevelInstant.java index f8257efe..fc0dfb7f 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevelInstant.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevelInstant.java @@ -2,13 +2,13 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ElevatorConstants.ElevatorPositions; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; public class SetElevatorLevelInstant extends Command { - private final ElevatorSubsystem elevatorSubsystem; + private final ElevatorSubsystemMD2 elevatorSubsystem; private final ElevatorPositions pos; - public SetElevatorLevelInstant(ElevatorSubsystem subsystem, ElevatorPositions pos) { + public SetElevatorLevelInstant(ElevatorSubsystemMD2 subsystem, ElevatorPositions pos) { elevatorSubsystem = subsystem; this.pos = pos; addRequirements(elevatorSubsystem); @@ -19,7 +19,7 @@ public void initialize() {} @Override public void execute() { - elevatorSubsystem.elevateTo(pos); + elevatorSubsystem.setHeight(pos.height); } @Override diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java index c19e9668..f8762b73 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java @@ -2,13 +2,13 @@ import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; public class ZeroElevatorHardStop extends Command { - private final ElevatorSubsystem elevatorSubsystem; + private final ElevatorSubsystemMD2 elevatorSubsystem; private double timesExceededCurrent; - public ZeroElevatorHardStop(ElevatorSubsystem subsystem) { + public ZeroElevatorHardStop(ElevatorSubsystemMD2 subsystem) { elevatorSubsystem = subsystem; DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/running", false); addRequirements(elevatorSubsystem); @@ -34,10 +34,9 @@ public void execute() { public void end(boolean interrupted) { DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/running", false); if (!interrupted) { - elevatorSubsystem.resetElevatorPositionToZero(); + elevatorSubsystem.zeroElevator(); } elevatorSubsystem.resetCurrentLimits(); - elevatorSubsystem.elevatorHasBeenZeroed(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java index 4b265808..a3ec940f 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java @@ -3,13 +3,13 @@ import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ElevatorConstants.ElevatorPositions; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; public class ZeroElevatorToFFiltered extends Command { - private final ElevatorSubsystem elevatorSubsystem; + private final ElevatorSubsystemMD2 elevatorSubsystem; private double ticksAtPosition; - public ZeroElevatorToFFiltered(ElevatorSubsystem subsystem) { + public ZeroElevatorToFFiltered(ElevatorSubsystemMD2 subsystem) { elevatorSubsystem = subsystem; DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/running", false); addRequirements(elevatorSubsystem); @@ -19,7 +19,7 @@ public ZeroElevatorToFFiltered(ElevatorSubsystem subsystem) { public void initialize() { ticksAtPosition = 0; DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/running", true); - elevatorSubsystem.elevateTo(ElevatorPositions.Intake); + elevatorSubsystem.setHeight(ElevatorPositions.Intake.height); } @Override @@ -34,7 +34,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/ticksAtPosition", ticksAtPosition); - boolean inPosition = elevatorSubsystem.isAtPosition() && elevatorSubsystem.atIntake(); + boolean inPosition = elevatorSubsystem.isAtTargetHeight() && elevatorSubsystem.atIntake(); DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/inPosition", inPosition); if (inPosition) { diff --git a/src/main/java/frc/robot/commands/FunnelCommands/RampUpFunnel.java b/src/main/java/frc/robot/commands/FunnelCommands/RampUpFunnel.java index 31e9f1cf..5230a400 100644 --- a/src/main/java/frc/robot/commands/FunnelCommands/RampUpFunnel.java +++ b/src/main/java/frc/robot/commands/FunnelCommands/RampUpFunnel.java @@ -2,15 +2,15 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.FunnelSubsystem; public class RampUpFunnel extends Command { private FunnelSubsystem funnelSubsystem; - private ElevatorSubsystem elevatorSubsystem; + private ElevatorSubsystemMD2 elevatorSubsystem; private double tolerance = 2; - public RampUpFunnel(FunnelSubsystem funnelSubsystem, ElevatorSubsystem elevator) { + public RampUpFunnel(FunnelSubsystem funnelSubsystem, ElevatorSubsystemMD2 elevator) { this.funnelSubsystem = funnelSubsystem; this.elevatorSubsystem = elevator; addRequirements(funnelSubsystem); @@ -26,7 +26,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (elevatorSubsystem.isAtPosition()) { + if (elevatorSubsystem.isAtTargetHeight()) { if (funnelSubsystem.getSpeed() > Constants.FunnelConstants.RAMP_UP_SPEED - tolerance && funnelSubsystem.getSpeed() < Constants.FunnelConstants.RAMP_UP_SPEED + tolerance) { funnelSubsystem.runFunnelAtRPS(Constants.FunnelConstants.RAMP_UP_SPEED); diff --git a/src/main/java/frc/robot/commands/FunnelCommands/RunFunnelUntilDetectionSafe.java b/src/main/java/frc/robot/commands/FunnelCommands/RunFunnelUntilDetectionSafe.java index 2d07180b..73726b58 100644 --- a/src/main/java/frc/robot/commands/FunnelCommands/RunFunnelUntilDetectionSafe.java +++ b/src/main/java/frc/robot/commands/FunnelCommands/RunFunnelUntilDetectionSafe.java @@ -7,7 +7,7 @@ point, the funnel maintains its current position (if the Coral moves the Funnel import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.CoralPosition; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.FunnelSubsystem; /** @@ -17,9 +17,10 @@ point, the funnel maintains its current position (if the Coral moves the Funnel */ public class RunFunnelUntilDetectionSafe extends Command { private FunnelSubsystem funnelSubsystem; - private ElevatorSubsystem elevatorSubsystem; + private ElevatorSubsystemMD2 elevatorSubsystem; - public RunFunnelUntilDetectionSafe(FunnelSubsystem funnelSubsystem, ElevatorSubsystem elevator) { + public RunFunnelUntilDetectionSafe( + FunnelSubsystem funnelSubsystem, ElevatorSubsystemMD2 elevator) { this.funnelSubsystem = funnelSubsystem; this.elevatorSubsystem = elevator; addRequirements(funnelSubsystem); @@ -36,7 +37,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (elevatorSubsystem.isAtPosition()) { + if (elevatorSubsystem.isAtTargetHeight()) { funnelSubsystem.spinFunnel(); } else { funnelSubsystem.maintainCurrentPosition(); diff --git a/src/main/java/frc/robot/commands/TransferPieceBetweenFunnelAndElevator.java b/src/main/java/frc/robot/commands/TransferPieceBetweenFunnelAndElevator.java index a57d9a38..254f66bd 100644 --- a/src/main/java/frc/robot/commands/TransferPieceBetweenFunnelAndElevator.java +++ b/src/main/java/frc/robot/commands/TransferPieceBetweenFunnelAndElevator.java @@ -2,19 +2,19 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.CoralPosition; -import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystemMD2; import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.TootsieSlideSubsystem; /* 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 TransferPieceBetweenFunnelAndElevator extends Command { - private ElevatorSubsystem elevatorSubsystem; + private ElevatorSubsystemMD2 elevatorSubsystem; private FunnelSubsystem funnelSubsystem; private TootsieSlideSubsystem tootsieSlideSubsystem; public TransferPieceBetweenFunnelAndElevator( - ElevatorSubsystem elevatorSubsystem, + ElevatorSubsystemMD2 elevatorSubsystem, FunnelSubsystem funnelSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem) { this.elevatorSubsystem = elevatorSubsystem; @@ -31,7 +31,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (elevatorSubsystem.isAtPosition()) { + if (elevatorSubsystem.isAtTargetHeight()) { funnelSubsystem.spinFunnel(); tootsieSlideSubsystem.intakeCoral(); } else { diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java new file mode 100644 index 00000000..ef326d7b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java @@ -0,0 +1,152 @@ +// 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 frc.robot.subsystems; + +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; +import dev.doglog.DogLog; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.ElevatorConstants; +import frc.robot.Constants.ElevatorConstants.ElevatorPositions; +import frc.robot.util.LoggedTalonFX; + +public class ElevatorSubsystemMD2 extends SubsystemBase { + private static ElevatorSubsystemMD2 instance; + private boolean elevatorZeroed; + + LoggedTalonFX motor1; + LoggedTalonFX motor2; + LoggedTalonFX master; + + MotionMagicVoltage request = new MotionMagicVoltage(null); + TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0); + VelocityVoltage velocityRequest = new VelocityVoltage(0); + + private double targetHeight; + private double currentHeightToF; + private ElevatorPositions currentLevel; + private double tolerance = 3.0; + + public ElevatorSubsystemMD2() { + elevatorZeroed = false; + motor1 = new LoggedTalonFX(1); + motor2 = new LoggedTalonFX(2); + currentLevel = ElevatorPositions.Intake; + + master = motor1; + Follower follower = new Follower(1, false); + motor2.setControl(follower); + + Slot0Configs s0c = new Slot0Configs().withKP(1.0).withKI(1.0).withKD(1.0); + + motor1.updateCurrentLimits(1.0, 1.0); + motor2.updateCurrentLimits(1.0, 1.0); + + MotionMagicConfigs mmc = + new MotionMagicConfigs() + .withMotionMagicAcceleration(ElevatorConstants.ACCELERATION) + .withMotionMagicCruiseVelocity(ElevatorConstants.CRUISE_VELOCITY); + + TalonFXConfigurator m1Config = motor1.getConfigurator(); + + m1Config.apply(s0c); + m1Config.apply(mmc); + } + + public void setHeight(double height) { + targetHeight = height; + master.setControl(request.withPosition(targetHeight)); + } + + public double getCurrentHeight() { + return master.getPosition().getValueAsDouble(); + } + + public double getTargetHeight() { + return targetHeight; + } + + public double getErrorDist() { + return Math.abs(targetHeight - getCurrentHeight()); + } + + public boolean isAtTargetHeight() { + return getErrorDist() <= tolerance; + } + + public void zeroElevator() { + master.setPosition(0); + } + + public void ElevatorTorqueMode() { + master.setControl(torqueRequest.withOutput(Constants.ElevatorConstants.ELEVATOR_TORQUE)); + } + + public void reduceCurrentLimits() { + master.updateCurrentLimits(30, 10); + } + + public void moveElevatorNegative() { + master.setControl(velocityRequest.withVelocity(-5).withSlot(1)); + } + + public void resetCurrentLimits() { + master.updateCurrentLimits( + Constants.ElevatorConstants.STATOR_CURRENT_LIMIT, + Constants.ElevatorConstants.SUPPLY_CURRENT_LIMIT); + } + + public boolean checkCurrent() { + double Supplycurrent = Math.abs(master.getSupplyCurrent().getValue().magnitude()); + double Statorcurrent = Math.abs(master.getStatorCurrent().getValue().magnitude()); + + if (Supplycurrent > 1.0 && Statorcurrent > 20) { + return true; + } + return false; + } + + public void resetPositionFiltered() { + master.setPosition( + currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + } + + public boolean atIntake() { + return currentLevel.equals(ElevatorPositions.Intake); + } + + public static ElevatorSubsystemMD2 getInstance() { + if (instance == null) { + instance = new ElevatorSubsystemMD2(); + } + return instance; + } + + public boolean isElevatorZeroed() { + return elevatorZeroed; + } + + public void elevatorHasBeenZeroed() { + elevatorZeroed = true; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + DogLog.log("Current angle", getCurrentHeight()); + DogLog.log("Is at target", isAtTargetHeight()); + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index 5004edc8..90800952 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -72,7 +72,7 @@ public class VisionSystem extends SubsystemBase { private Constants.Vision.Cameras cameraEnum; private PhotonPipelineResult pipeline; AprilTagFieldLayout aprilTagFieldLayout = - AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); + AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); PhotonPoseEstimator photonPoseEstimator; private SwerveSubsystem driveTrain = SwerveSubsystem.getInstance(); private BooleanSupplier redSide; From a6e9cf086ebe69e282ae0c1004e1dc73200e21c3 Mon Sep 17 00:00:00 2001 From: Parneel Bhakhri Date: Wed, 8 Oct 2025 19:18:52 -0700 Subject: [PATCH 2/5] remove hardcoded values --- .../subsystems/ElevatorSubsystemMD2.java | 55 +++++++++++++++++-- 1 file changed, 49 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java index ef326d7b..093b4779 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java @@ -5,12 +5,18 @@ package frc.robot.subsystems; import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; + import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -45,20 +51,57 @@ public ElevatorSubsystemMD2() { Follower follower = new Follower(1, false); motor2.setControl(follower); - Slot0Configs s0c = new Slot0Configs().withKP(1.0).withKI(1.0).withKD(1.0); - motor1.updateCurrentLimits(1.0, 1.0); motor2.updateCurrentLimits(1.0, 1.0); - MotionMagicConfigs mmc = - new MotionMagicConfigs() - .withMotionMagicAcceleration(ElevatorConstants.ACCELERATION) - .withMotionMagicCruiseVelocity(ElevatorConstants.CRUISE_VELOCITY); + MotionMagicConfigs mmc = new MotionMagicConfigs() + .withMotionMagicAcceleration(ElevatorConstants.ACCELERATION) + .withMotionMagicCruiseVelocity(ElevatorConstants.CRUISE_VELOCITY); TalonFXConfigurator m1Config = motor1.getConfigurator(); + TalonFXConfigurator m2Config = motor2.getConfigurator(); + + Slot1Configs s1c = new Slot1Configs() + .withKP(ElevatorConstants.S1C_KP) + .withKI(ElevatorConstants.S1C_KI) + .withKD(ElevatorConstants.S1C_KD) + .withKS(ElevatorConstants.S0C_KS) + .withKG(ElevatorConstants.S0C_KG) + .withKA(ElevatorConstants.S0C_KA) + .withKV(ElevatorConstants.S0C_KV) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + Slot0Configs s0c = new Slot0Configs() + .withKP(ElevatorConstants.S0C_KP) + .withKI(ElevatorConstants.S0C_KI) + .withKD(ElevatorConstants.S0C_KD) + .withKS(ElevatorConstants.S0C_KS) + .withKG(ElevatorConstants.S0C_KG) + .withKA(ElevatorConstants.S0C_KA) + .withKV(ElevatorConstants.S0C_KV) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); m1Config.apply(s0c); + m2Config.apply(s0c); + m1Config.apply(s1c); + m2Config.apply(s1c); + + MotorOutputConfigs moc = new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake); + + mmc = new MotionMagicConfigs(); + mmc.MotionMagicCruiseVelocity = ElevatorConstants.MOTIONMAGIC_MAX_VELOCITY; + mmc.MotionMagicAcceleration = ElevatorConstants.MOTIONMAGIC_MAX_ACCELERATION; + m1Config.apply(mmc); + m2Config.apply(mmc); + + m1Config.apply(moc); + m2Config.apply(moc); + + master = motor1; + resetPositionFiltered(); } public void setHeight(double height) { From f583bd31e3daf1f4c998fe093d111577be3c7b1d Mon Sep 17 00:00:00 2001 From: Parneel Bhakhri Date: Wed, 8 Oct 2025 19:43:41 -0700 Subject: [PATCH 3/5] fix null pointer reference --- .../subsystems/ElevatorSubsystemMD2.java | 54 ++++++++++--------- 1 file changed, 28 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java index 093b4779..3bc473ca 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java @@ -16,7 +16,6 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; - import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -32,7 +31,7 @@ public class ElevatorSubsystemMD2 extends SubsystemBase { LoggedTalonFX motor2; LoggedTalonFX master; - MotionMagicVoltage request = new MotionMagicVoltage(null); + MotionMagicVoltage request = new MotionMagicVoltage(0); TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0); VelocityVoltage velocityRequest = new VelocityVoltage(0); @@ -54,34 +53,37 @@ public ElevatorSubsystemMD2() { motor1.updateCurrentLimits(1.0, 1.0); motor2.updateCurrentLimits(1.0, 1.0); - MotionMagicConfigs mmc = new MotionMagicConfigs() - .withMotionMagicAcceleration(ElevatorConstants.ACCELERATION) - .withMotionMagicCruiseVelocity(ElevatorConstants.CRUISE_VELOCITY); + MotionMagicConfigs mmc = + new MotionMagicConfigs() + .withMotionMagicAcceleration(ElevatorConstants.ACCELERATION) + .withMotionMagicCruiseVelocity(ElevatorConstants.CRUISE_VELOCITY); TalonFXConfigurator m1Config = motor1.getConfigurator(); TalonFXConfigurator m2Config = motor2.getConfigurator(); - Slot1Configs s1c = new Slot1Configs() - .withKP(ElevatorConstants.S1C_KP) - .withKI(ElevatorConstants.S1C_KI) - .withKD(ElevatorConstants.S1C_KD) - .withKS(ElevatorConstants.S0C_KS) - .withKG(ElevatorConstants.S0C_KG) - .withKA(ElevatorConstants.S0C_KA) - .withKV(ElevatorConstants.S0C_KV) - .withGravityType(GravityTypeValue.Elevator_Static) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - - Slot0Configs s0c = new Slot0Configs() - .withKP(ElevatorConstants.S0C_KP) - .withKI(ElevatorConstants.S0C_KI) - .withKD(ElevatorConstants.S0C_KD) - .withKS(ElevatorConstants.S0C_KS) - .withKG(ElevatorConstants.S0C_KG) - .withKA(ElevatorConstants.S0C_KA) - .withKV(ElevatorConstants.S0C_KV) - .withGravityType(GravityTypeValue.Elevator_Static) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + Slot1Configs s1c = + new Slot1Configs() + .withKP(ElevatorConstants.S1C_KP) + .withKI(ElevatorConstants.S1C_KI) + .withKD(ElevatorConstants.S1C_KD) + .withKS(ElevatorConstants.S0C_KS) + .withKG(ElevatorConstants.S0C_KG) + .withKA(ElevatorConstants.S0C_KA) + .withKV(ElevatorConstants.S0C_KV) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + Slot0Configs s0c = + new Slot0Configs() + .withKP(ElevatorConstants.S0C_KP) + .withKI(ElevatorConstants.S0C_KI) + .withKD(ElevatorConstants.S0C_KD) + .withKS(ElevatorConstants.S0C_KS) + .withKG(ElevatorConstants.S0C_KG) + .withKA(ElevatorConstants.S0C_KA) + .withKV(ElevatorConstants.S0C_KV) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); m1Config.apply(s0c); m2Config.apply(s0c); From 8c6ea4900ea37a56dc899dce29cd95b38bc3c0e0 Mon Sep 17 00:00:00 2001 From: Parneel Bhakhri Date: Wed, 8 Oct 2025 20:57:52 -0700 Subject: [PATCH 4/5] changes --- src/main/java/frc/robot/Robot.java | 2 +- .../subsystems/ElevatorSubsystemMD2.java | 73 ++++++++++--------- 2 files changed, 40 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a41de565..03b129e6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -89,7 +89,7 @@ public void disabledInit() { @Override public void robotInit() { DogLog.setOptions( - new DogLogOptions().withNtPublish(false).withCaptureDs(true).withLogExtras(true)); + new DogLogOptions().withNtPublish(true).withCaptureDs(true).withLogExtras(true)); DogLog.log("PIDArmKP", Constants.Arm.S0C_KP); DogLog.log("PIDArmKI", Constants.Arm.S0C_KI); DogLog.log("PIDArmKD", Constants.Arm.S0C_KD); diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java index 3bc473ca..61c60e82 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java @@ -42,48 +42,49 @@ public class ElevatorSubsystemMD2 extends SubsystemBase { public ElevatorSubsystemMD2() { elevatorZeroed = false; - motor1 = new LoggedTalonFX(1); - motor2 = new LoggedTalonFX(2); + motor1 = new LoggedTalonFX( + "subsystems/Elevator/motor1", + ElevatorConstants.MOTOR1_PORT, + Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); + motor2 = new LoggedTalonFX("subsystems/Elevator/motor2", + ElevatorConstants.MOTOR2_PORT, + Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); currentLevel = ElevatorPositions.Intake; master = motor1; - Follower follower = new Follower(1, false); + Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, false); motor2.setControl(follower); - motor1.updateCurrentLimits(1.0, 1.0); - motor2.updateCurrentLimits(1.0, 1.0); + motor1.updateCurrentLimits(ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); - MotionMagicConfigs mmc = - new MotionMagicConfigs() - .withMotionMagicAcceleration(ElevatorConstants.ACCELERATION) - .withMotionMagicCruiseVelocity(ElevatorConstants.CRUISE_VELOCITY); + MotionMagicConfigs mmc = new MotionMagicConfigs() + .withMotionMagicAcceleration(ElevatorConstants.ACCELERATION) + .withMotionMagicCruiseVelocity(ElevatorConstants.CRUISE_VELOCITY); TalonFXConfigurator m1Config = motor1.getConfigurator(); TalonFXConfigurator m2Config = motor2.getConfigurator(); - Slot1Configs s1c = - new Slot1Configs() - .withKP(ElevatorConstants.S1C_KP) - .withKI(ElevatorConstants.S1C_KI) - .withKD(ElevatorConstants.S1C_KD) - .withKS(ElevatorConstants.S0C_KS) - .withKG(ElevatorConstants.S0C_KG) - .withKA(ElevatorConstants.S0C_KA) - .withKV(ElevatorConstants.S0C_KV) - .withGravityType(GravityTypeValue.Elevator_Static) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - - Slot0Configs s0c = - new Slot0Configs() - .withKP(ElevatorConstants.S0C_KP) - .withKI(ElevatorConstants.S0C_KI) - .withKD(ElevatorConstants.S0C_KD) - .withKS(ElevatorConstants.S0C_KS) - .withKG(ElevatorConstants.S0C_KG) - .withKA(ElevatorConstants.S0C_KA) - .withKV(ElevatorConstants.S0C_KV) - .withGravityType(GravityTypeValue.Elevator_Static) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + Slot1Configs s1c = new Slot1Configs() + .withKP(ElevatorConstants.S1C_KP) + .withKI(ElevatorConstants.S1C_KI) + .withKD(ElevatorConstants.S1C_KD) + .withKS(ElevatorConstants.S0C_KS) + .withKG(ElevatorConstants.S0C_KG) + .withKA(ElevatorConstants.S0C_KA) + .withKV(ElevatorConstants.S0C_KV) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + Slot0Configs s0c = new Slot0Configs() + .withKP(ElevatorConstants.S0C_KP) + .withKI(ElevatorConstants.S0C_KI) + .withKD(ElevatorConstants.S0C_KD) + .withKS(ElevatorConstants.S0C_KS) + .withKG(ElevatorConstants.S0C_KG) + .withKA(ElevatorConstants.S0C_KA) + .withKV(ElevatorConstants.S0C_KV) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); m1Config.apply(s0c); m2Config.apply(s0c); @@ -107,7 +108,7 @@ public ElevatorSubsystemMD2() { } public void setHeight(double height) { - targetHeight = height; + targetHeight = height * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS; master.setControl(request.withPosition(targetHeight)); } @@ -123,8 +124,12 @@ public double getErrorDist() { return Math.abs(targetHeight - getCurrentHeight()); } + // public boolean isAtTargetHeight() { + // return getErrorDist() <= tolerance; + // } + public boolean isAtTargetHeight() { - return getErrorDist() <= tolerance; + return (Math.abs(getErrorDist()) <= ElevatorConstants.SETPOINT_TOLERANCE); } public void zeroElevator() { From ba05c30777a30aace7853f1dd14a812c68812c6e Mon Sep 17 00:00:00 2001 From: Parneel Bhakhri Date: Wed, 15 Oct 2025 19:04:26 -0700 Subject: [PATCH 5/5] it works now --- .../robot/subsystems/ElevatorSubsystemMD2.java | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java index 61c60e82..69aa8f6f 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java @@ -46,7 +46,8 @@ public ElevatorSubsystemMD2() { "subsystems/Elevator/motor1", ElevatorConstants.MOTOR1_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); - motor2 = new LoggedTalonFX("subsystems/Elevator/motor2", + motor2 = new LoggedTalonFX( + "subsystems/Elevator/motor2", ElevatorConstants.MOTOR2_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); currentLevel = ElevatorPositions.Intake; @@ -55,7 +56,8 @@ public ElevatorSubsystemMD2() { Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, false); motor2.setControl(follower); - motor1.updateCurrentLimits(ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); + motor1.updateCurrentLimits( + ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); MotionMagicConfigs mmc = new MotionMagicConfigs() .withMotionMagicAcceleration(ElevatorConstants.ACCELERATION) @@ -108,7 +110,8 @@ public ElevatorSubsystemMD2() { } public void setHeight(double height) { - targetHeight = height * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS; + targetHeight = height * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS + / ElevatorConstants.CARRAIGE_UPDUCTION; master.setControl(request.withPosition(targetHeight)); } @@ -125,7 +128,7 @@ public double getErrorDist() { } // public boolean isAtTargetHeight() { - // return getErrorDist() <= tolerance; + // return getErrorDist() <= tolerance; // } public boolean isAtTargetHeight() { @@ -191,8 +194,9 @@ public void elevatorHasBeenZeroed() { @Override public void periodic() { // This method will be called once per scheduler run - DogLog.log("Current angle", getCurrentHeight()); - DogLog.log("Is at target", isAtTargetHeight()); + DogLog.log("subsystems/ElevatorMD2/currentHeight", getCurrentHeight()); + DogLog.log("subsystems/ElevatorMD2/isAtTargetHeight", isAtTargetHeight()); + DogLog.log("subsystems/ElevatorMD2/targetHeight", targetHeight); } @Override