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/Constants.java b/src/main/java/frc/robot/Constants.java index 7ef43561..ac1abeb2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -871,6 +871,7 @@ public static class FunnelConstants { } public static class ElevatorConstants { + public static final int MOTOR1_PORT = 11; // TODO: change port public static final int MOTOR2_PORT = 12; // TODO: change port public static final int CANRANGE_PORT = 41; // TODO: change port diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1743b9ec..03b129e6 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; @@ -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); @@ -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..dd0ceb12 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevel.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevel.java @@ -3,15 +3,16 @@ 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) { + ElevatorSubsystemMD2 subsystem, ElevatorPositions pos, boolean checkIfCoralInTootsie) { + elevatorSubsystem = subsystem; this.pos = pos; this.checkIfCoralInTootsie = checkIfCoralInTootsie; @@ -25,10 +26,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 +38,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..a279a2b2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystemMD2.java @@ -0,0 +1,219 @@ +// 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.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; +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; + + // config: wip, will update based on last years elevator subsystem before testing + + LoggedTalonFX motor1; + LoggedTalonFX motor2; + LoggedTalonFX master; + + MotionMagicVoltage request = new MotionMagicVoltage(0); + + private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0); + private final VelocityVoltage velocityRequest = new VelocityVoltage(0); + + private double currentHeightToF; + private double targetHeight; + + public ElevatorSubsystemMD2() { + 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); + + Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, false); + motor2.setControl(follower); + + 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); + + motor1.updateCurrentLimits( + ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); + motor2.updateCurrentLimits( + ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); + + TalonFXConfigurator m1Config = motor1.getConfigurator(); + TalonFXConfigurator m2Config = motor2.getConfigurator(); + + m1Config.apply(s0c); + m1Config.apply(s1c); + m2Config.apply(s0c); + m2Config.apply(s1c); + + MotorOutputConfigs moc = new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake); + + MotionMagicConfigs mmc = + new MotionMagicConfigs() + .withMotionMagicAcceleration(ElevatorConstants.MOTIONMAGIC_MAX_ACCELERATION) + .withMotionMagicCruiseVelocity(ElevatorConstants.MOTIONMAGIC_MAX_VELOCITY); + + m1Config.apply(mmc); + m2Config.apply(mmc); + m1Config.apply(moc); + m2Config.apply(moc); + + master = motor1; + } + + public static ElevatorSubsystemMD2 getInstance() { + if (instance == null) { + instance = new ElevatorSubsystemMD2(); + } + return instance; + } + + // use a control request to move to the height. + public void setHeight(double position) { + + targetHeight = position * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS / ElevatorConstants.CARRAIGE_UPDUCTION; + master.setControl(request.withPosition(targetHeight)); + + master.setControl( + request + .withPosition(targetHeight) + .withSlot(0)); + } + + // getters + public double getCurrentHeight() { + return master.getPosition().getValueAsDouble(); + } + + public double getTargetHeight() { + return targetHeight; + } + + // distance from the target + public double getErrorDist() { + return Math.abs(targetHeight - getCurrentHeight()); + } + + public boolean isAtTargetHeight() { + return getErrorDist() <= ElevatorConstants.SETPOINT_TOLERANCE; + } + + // based on last year's code + public boolean atIntake() { + return getCurrentHeight() == (ElevatorPositions.Intake.height); + } + + public void zeroElevator() { + master.setPosition(0); + } + + public boolean isElevatorZeroed() { + return master.getPosition().getValueAsDouble() == 0; + } + + public void reduceCurrentLimits() { + master.updateCurrentLimits(30, 10); + } + + // all of these created based on last year's code for one of the commands that requires it, my + // interpretation of their use is listed + + // move elevator down, i assume this is for re-zeroing it. + public void moveElevatorNegative() { + master.setControl(velocityRequest.withVelocity(-5).withSlot(1)); + } + + // set torque mode from constants. + public void ElevatorTorqueMode() { + DogLog.log("subsystems/Elevator/usingTorqueMode", true); + master.setControl(torqueRequest.withOutput(Constants.ElevatorConstants.ELEVATOR_TORQUE)); + // .withMaxAbsDutyCycle(Constants.ElevatorConstants.ELEVATOR_DUTY_CYCLE)); + } + + // apply the current limits defined in constants + public void resetCurrentLimits() { + master.updateCurrentLimits( + Constants.ElevatorConstants.STATOR_CURRENT_LIMIT, + Constants.ElevatorConstants.SUPPLY_CURRENT_LIMIT); + } + + // log the supply and stator current, return bool depending if they are above certain thresholds. + public boolean checkCurrent() { + double Supplycurrent = Math.abs(master.getSupplyCurrent().getValue().magnitude()); + double Statorcurrent = Math.abs(master.getStatorCurrent().getValue().magnitude()); + DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/supply", Supplycurrent); + DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/stator", Statorcurrent); + + if (Supplycurrent > 1.0 && Statorcurrent > 20) { + return true; + } + return false; + } + + // set the position based on tof sensor + public void resetPositionFiltered() { + master.setPosition( + currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + DogLog.log( + "subsystems/Elevator/resetElevatorPosition", + currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + DogLog.log("subsystems/ElevatorMD2/currentHeight", getCurrentHeight()); + DogLog.log("subsystems/ElevatorMD2/targetHeight", isAtTargetHeight()); + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +}