Pre cmp/automated ops check

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 {

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

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());

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()
+        );
+    }
 }

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);
     }

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;