diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0d058dbf..7241d3e6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -38,10 +38,12 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Time; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import frc.robot.generated.TunerConstants; import frc.util.AllianceFlipUtil; import frc.util.VisionUtil; @@ -258,6 +260,9 @@ public static class ShooterK { private static final VoltageConfigs kHoodVoltageConfigs = new VoltageConfigs() .withPeakForwardVoltage(16) .withPeakReverseVoltage(-16); + private static final SoftwareLimitSwitchConfigs kHoodSoftwareLimitSwitchConfigs = new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(kHoodMaxRots_double); private static final CommutationConfigs kHoodCommutationConfigs = new CommutationConfigs() .withAdvancedHallSupport(AdvancedHallSupportValue.Enabled) .withMotorArrangement(MotorArrangementValue.NEO550_JST); @@ -409,6 +414,11 @@ public static class RobotK { public static final int kMiniPCChannel = 14; + public static final LinearVelocity kMaxTranslationSpeed = TunerConstants.kSpeedAt12Volts; // kSpeedAt12Volts desired top speed + public static final AngularVelocity kMaxAngularRate = RotationsPerSecond.of(1.05); // 3/4 of a rotation per second max angular velocity + + public static final LinearVelocity kTranslationOpsCheckSpeed = MetersPerSecond.of(0.1); + // real values public static final Distance kRobotFullWidth = Inches.of(33.6875); public static final Distance kRobotFullLength = Inches.of(32.6875); @@ -419,6 +429,12 @@ public static class RobotK { public static class SuperstructureK { public static final String kLogTab = "Superstructure"; + + public static final double kLongOpsCheckPause = 4; //in secs + public static final double kShortOpsCheckPause = 2.5; //in secs + + public static final double kShortOpsCheckShooterTime = 5; + public static final double kShortOpsCheckIntakeTime = 5; } public static class IntakeK { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ab914bd8..e5dbab53 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -20,6 +20,7 @@ import choreo.auto.AutoFactory; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DataLogManager; @@ -62,8 +63,6 @@ public class Robot extends TimedRobot { /* CLASS VARIABLES */ //---CONSTANTS - private final LinearVelocity kMaxTranslationSpeed = TunerConstants.kSpeedAt12Volts; // kSpeedAt12Volts desired top speed - private final AngularVelocity kMaxAngularRate = RotationsPerSecond.of(1.05); // 3/4 of a rotation per second max angular velocity // Pre-computed doubles for driveCommand hot path (avoids .times() measure allocations every tick) private final double kMaxTranslationMps = kMaxTranslationSpeed.in(MetersPerSecond); @@ -101,7 +100,7 @@ public class Robot extends TimedRobot { private final Indexer m_indexer = new Indexer(); // private final WaltVisualSim m_visualSim; - private final Superstructure m_superstructure = new Superstructure(m_intake, m_indexer, m_shooter); + private final Superstructure m_superstructure = new Superstructure(m_intake, m_indexer, m_shooter, m_drivetrain); //---AUTONS private final AutoFactory m_autoFactory = m_drivetrain.createAutoFactory(); @@ -344,6 +343,11 @@ private void configureBindings() { m_driver.povDown().onTrue(m_shooter.m_turret.setTurretLockCmd(false)); m_driver.povRight().onTrue(m_shooter.m_turret.setTurretLockCmd(true)); + + // m_driver.povDown().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX(), m_drivetrain.getState().Pose.getY() - Inches.of(20).magnitude()), 0.001)); + // m_driver.povUp().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX(), m_drivetrain.getState().Pose.getY() + Inches.of(20).magnitude()), 0.001)); + // m_driver.povRight().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX() + Inches.of(20).magnitude(), m_drivetrain.getState().Pose.getY()), 0.001)); + // m_driver.povLeft().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX() - Inches.of(20).magnitude(), m_drivetrain.getState().Pose.getY()), 0.001)); m_manipulator.y().and(trg_manipOverride).onTrue(Commands.runOnce(() -> m_shooter.m_turret.homeTurret(true))); @@ -497,43 +501,7 @@ public void teleopExit() {} @Override public void testInit() { - CommandScheduler.getInstance().cancelAll(); - - CommandScheduler.getInstance().schedule( - Commands.sequence( - m_drivetrain.runOnce(m_drivetrain::seedFieldCentric), - Commands.waitSeconds(1), - m_drivetrain.applyRequest(() -> - drive.withVelocityX(kMaxTranslationSpeed) - .withVelocityY(0) - .withRotationalRate(0) - ), - Commands.waitSeconds(2.5), - m_drivetrain.xBrakeCmd(), - Commands.waitSeconds(2.5), - m_drivetrain.applyRequest(() -> - drive.withVelocityX(kMaxTranslationSpeed.unaryMinus()) - .withVelocityY(0) - .withRotationalRate(0) - ), - Commands.waitSeconds(2.5), - m_drivetrain.xBrakeCmd(), - Commands.waitSeconds(2.5), - m_drivetrain.applyRequest(() -> - drive.withVelocityX(0) - .withVelocityY(0) - .withRotationalRate(kMaxAngularRate) - ), - Commands.waitSeconds(2.5), - m_drivetrain.xBrakeCmd(), - Commands.waitSeconds(2.5), - m_drivetrain.applyRequest(() -> - drive.withVelocityX(0) - .withVelocityY(0) - .withRotationalRate(0) - ) - ) - ); + CommandScheduler.getInstance().schedule(m_superstructure.longOpsCheck()); } @Override diff --git a/src/main/java/frc/robot/dashboards/TestingDashboard.java b/src/main/java/frc/robot/dashboards/TestingDashboard.java index e8416c32..ff9cc028 100644 --- a/src/main/java/frc/robot/dashboards/TestingDashboard.java +++ b/src/main/java/frc/robot/dashboards/TestingDashboard.java @@ -39,6 +39,8 @@ public class TestingDashboard { // public static BooleanTopic BT_letIntakeArmPositionRotsChange = nte_inst.getBooleanTopic("/TestingDashboard/letIntakeArmPositionRotsChange"); // public static BooleanTopic BT_letIntakeRollersVelocityRPSChange = nte_inst.getBooleanTopic("/TestingDashboard/letIntakeRollersVelocityRPSChange"); + public static BooleanTopic BT_letOpsCheckBeLong = nte_inst.getBooleanTopic("/TestingDashboard/letOpsCheckBeLong"); + // /* PUBLISHERS */ // //---SHOOTER public static DoublePublisher pub_shooterVelocityRPS; @@ -64,6 +66,9 @@ public class TestingDashboard { // public static BooleanPublisher pub_letIntakeArmPositionRotsChange; // public static BooleanPublisher pub_letIntakeRollersVelocityRPSChange; + //---SELECT OPS CHECK SWITCHES + public static BooleanPublisher pub_letOpsCheckBeLong; + // /* SUBSCRIBERS */ // //---SHOOTER public static DoubleSubscriber sub_shooterVelocityRPS; @@ -89,6 +94,9 @@ public class TestingDashboard { // public static BooleanSubscriber sub_letIntakeArmPositionRotsChange; // public static BooleanSubscriber sub_letIntakeRollersVelocityRPSChange; + //---SELECT OPS CHECK SWITCHES + public static BooleanSubscriber sub_letOpsCheckBeLong; + /* TRIGGERS */ public static Trigger trg_letShooterVelocityRPSChange; // public static Trigger trg_letTurretPositionRotsChange; @@ -100,6 +108,8 @@ public class TestingDashboard { // public static Trigger trg_letIntakeArmPositionRotsChange; // public static Trigger trg_letIntakeRollersVelocityRPSChange; + // public static Trigger trg_letOpsCheckBeLong; + public static void initialize() { //---SHOOTER pub_shooterVelocityRPS = DT_shooterVelocityRPS.publish(); @@ -159,6 +169,13 @@ public static void initialize() { // sub_letIntakeArmPositionRotsChange = BT_letIntakeArmPositionRotsChange.subscribe(false); // sub_letIntakeRollersVelocityRPSChange = BT_letIntakeRollersVelocityRPSChange.subscribe(false); + //--SELECT OPS CHECK SWITCHES + pub_letOpsCheckBeLong = BT_letOpsCheckBeLong.publish(); + + pub_letOpsCheckBeLong.setDefault(false); + + sub_letOpsCheckBeLong = BT_letOpsCheckBeLong.subscribe(false); + //---TRIGGERS trg_letShooterVelocityRPSChange = new Trigger(() -> sub_letShooterVelocityRPSChange.get()); // trg_letTurretPositionRotsChange = new Trigger(() -> sub_letTurretPositionRotsChange.get()); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index d7bdefc0..a7fa8acd 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -5,8 +5,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.IndexerK; import frc.robot.Constants.IntakeK; +import frc.robot.Constants.SuperstructureK; import frc.robot.subsystems.Intake.IntakeArmPosition; import frc.robot.subsystems.shooter.Shooter; import static edu.wpi.first.units.Units.RotationsPerSecond; @@ -19,12 +21,16 @@ public class Superstructure extends SubsystemBase { private final Intake m_intake; private final Indexer m_indexer; private final Shooter m_shooter; + private final Swerve m_drivetrain; + + public boolean m_isLongOpsCheck = true; /* CONSTRUCTOR */ - public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { + public Superstructure(Intake intake, Indexer indexer, Shooter shooter, Swerve drivetrain) { m_intake = intake; m_indexer = indexer; m_shooter = shooter; + m_drivetrain = drivetrain; } /* BUTTON BIND SEQUENCES */ @@ -211,4 +217,82 @@ public Command intakeTo(IntakeArmPosition pos) { m_intake.setIntakeArmPosCmd(pos) ); } + + /** + * runs each subsytem in order of how the ball will flow through the robot (intake -> spindxer -> tunnel -> shooter -> swerve) + * and then everything runs together like they would in a match + * + * @return an automated ops check that runs each subsystem individually + */ + public Command longOpsCheck() { + return Commands.sequence( + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + + //deploy intake + m_intake.setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + + //run intake rollers + m_intake.startIntakeRollers(), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + + //run spindexer + m_intake.stopIntakeRollers(), + m_indexer.setSpindexerVelocityCmd(IndexerK.kSpindexerShootRPS), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + + //Stop Spindexer; run tunnel + m_indexer.stopSpindexerCmd(), + m_indexer.setTunnelVelocityCmd(IndexerK.kTunnelShootRPS), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + + //Stop tunnel; run shooter + m_indexer.stopTunnelCmd(), + m_shooter.setShooterVelocityCmd(ShooterK.kShooterRPS), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + + //shooter stopped + m_shooter.setShooterVelocityCmd(RotationsPerSecond.zero()), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + + //turret move + m_shooter.m_turret.setTurretLockCmd(true), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + m_shooter.m_turret.setTurretLockCmd(true), + + //hood move + m_shooter.m_hood.setHoodPosCmd(ShooterK.kHoodLockRots_double), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + m_shooter.m_hood.setHoodPosCmd(0.1), + + //Run short Ops check (intake, shoot, swerve) + shortOpsCheck() + ); + } + + /** + * runs the intaking cmd – 5 seconds, + * the outaking cmd – 4 seconds, + * and then the swerve tests (run wheels forward, backward, and then rotates them) – 2.5 seconds between each test + * + * @return a sequence of commands that runs the subsystems in groups – how they would run together in a match + * (ex. intaking cmd runs intake arm and rollers, spindexer and turret) + */ + public Command shortOpsCheck() { + return Commands.sequence( + //runs intaking cmd + Commands.print("================================= ACTIVATE INTAKE CMD ================================="), + intake(() -> false).withTimeout(SuperstructureK.kShortOpsCheckIntakeTime), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + + //runs shooting cmd + Commands.print("================================= ACTIVATE OUTTAKE CMD ================================="), + activateOuttakeShotCalc().withTimeout(SuperstructureK.kShortOpsCheckShooterTime), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + + //Swerve test + Commands.print("================================= START SWERVE OPSP CHECK ================================="), + m_drivetrain.swerveAutomatedOpsCheck() + ); + } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index bf138500..0d68e57b 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -41,6 +41,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants.RobotK; +import frc.robot.Constants.SuperstructureK; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; import frc.robot.vision.Detection; import frc.util.WaltDriverStation; @@ -510,6 +512,58 @@ public static Pose2d faceFuelPose(Pose2d robotPose, Pose2d fuelLocation) { ); } + /** + * drive the robot forward slow, then fast + * backward slow, then fast + * then spin the wheels driverMax speed + * + * @return Ops check for swerve drive + */ + public Command swerveAutomatedOpsCheck() { + return Commands.sequence( + //Slow speed forward into high speed + applyRequest(() -> + swreq_drive.withVelocityX(RobotK.kTranslationOpsCheckSpeed) + .withVelocityY(0) + .withRotationalRate(0) + ), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + applyRequest(() -> + swreq_drive.withVelocityX(RobotK.kMaxTranslationSpeed) + .withVelocityY(0) + .withRotationalRate(0) + ), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + xBrakeCmd(), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + + //Slow speed backward into high speed + applyRequest(() -> + swreq_drive.withVelocityX(RobotK.kTranslationOpsCheckSpeed.times(-1)) + .withVelocityY(0) + .withRotationalRate(0) + ), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + applyRequest(() -> + swreq_drive.withVelocityX(RobotK.kMaxTranslationSpeed.times(-1)) + .withVelocityY(0) + .withRotationalRate(0) + ), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + xBrakeCmd(), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + + //Turn wheels + applyRequest(() -> + swreq_drive.withVelocityX(0) + .withVelocityY(0) + .withRotationalRate(RobotK.kMaxAngularRate) + ), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + xBrakeCmd() + ); + } + public boolean isBeached() { return sig_gyroRoll.refresh().isNear(0, 3) && sig_gyroYaw.refresh().isNear(0, 3); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 5c4c9d4f..79d4b4f0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity;