Pre dcmp/adding superstructure truth machine

src/main/java/frc/robot/Robot.java

@@ -111,9 +111,6 @@ public class Robot extends TimedRobot {
     private Trigger trg_manipOverride = m_manipulator.b();
 
     //---COMMAND SEQUENCE TRIGGERS
-
-    // private Trigger trg_activateIntake = m_manipulator.a().and(trg_manipOverride.negate());
-    // private Trigger trg_safeIntake = m_manipulator.x().and(trg_manipOverride.negate());
     private Trigger trg_intake = m_manipulator.rightTrigger().and(trg_manipOverride.negate());
     private Trigger trg_retractIntake = m_manipulator.rightBumper().and(trg_manipOverride.negate());
 
@@ -131,6 +128,10 @@ public class Robot extends TimedRobot {
 
     private Trigger trg_unjam = m_driver.rightBumper();
 
+    //---SAFETY TRIGGERS
+    private Trigger trg_canIntake = trg_emergencyBarf.negate();
+    private Trigger trg_canShoot = trg_emergencyBarf.negate();
+
 
     /* LOGGERS */
     private final DoubleLogger log_stickDesiredFieldX = WaltLogger.logDouble("Swerve", "stick desired teleop x");
@@ -264,8 +265,8 @@ private void configureBindings() {
 
         //---NORMAL SEQUENCES
         //Intake
-        trg_intake.and(trg_shoot.negate()).whileTrue(
-            m_superstructure.intake(() -> false)
+        trg_intake.and(trg_canIntake).whileTrue(
+            m_superstructure.intakeCmd(() -> trg_shoot.getAsBoolean())
         );
 
         trg_retractIntake.onTrue(
@@ -275,7 +276,7 @@ private void configureBindings() {
         //Shooting
         // NORMAL FIXED SHOT
         // trg_shoot.whileTrue(m_superstructure.activateOuttake(() -> RotationsPerSecond.of(TestingDashboard.sub_shooterVelocityRPS.get())));
-        trg_shoot.whileTrue(m_superstructure.activateOuttakeShotCalc());    //comment out for LERP with above
+        trg_shoot.and(trg_canShoot).whileTrue(m_superstructure.shootWithShotCalcCmd());    //comment out for LERP with above
 
         // m_driver.y().onTrue(m_shooter.driverRPSAlter(true));
         // m_driver.a().onTrue(m_shooter.driverRPSAlter(false));
@@ -290,22 +291,14 @@ private void configureBindings() {
         // snapshot on each shoot press
         trg_shoot.onTrue(WaltCamera.takeSnapshotCmd());
 
-        trg_intake.and(trg_shoot).whileTrue(
-            m_superstructure.intake(() -> true)
-        );
-
         trg_emergencyBarf.whileTrue(
-            m_superstructure.emergencyBarf()
+            m_superstructure.emergencyBarfCmd()
         );
 
-        trg_shimmy.whileTrue(m_superstructure.shimmy());
+        trg_shimmy.whileTrue(m_superstructure.intakeArmShimmyCmd());
 
-        trg_unjam.and(trg_shoot.negate()).whileTrue(
-            m_superstructure.unjamCmd(() -> false)
-        );
-
-        trg_unjam.and(trg_shoot).whileTrue(
-            m_superstructure.unjamCmd(()-> true)
+        trg_unjam.whileTrue(
+            m_superstructure.unjamCmd(() -> trg_shoot.getAsBoolean())
         );
 
         //---OVERRIDE COMMANDS
@@ -315,18 +308,18 @@ private void configureBindings() {
         // m_manipulator.a().and(trg_manipOverride).onTrue(m_shooter.setHoodPositionCmd(Degrees.of(1)));
 
         trg_deployIntakeOverride.onTrue(
-            m_superstructure.intakeTo(IntakeArmPosition.DEPLOYED)
+            m_intake.setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED)
         ).onFalse(
-            m_superstructure.intakeTo(IntakeArmPosition.SAFE)
+            m_intake.setIntakeArmPosCmd(IntakeArmPosition.SAFE)
         );
         trg_intakeUpOverride.onTrue(
-            m_superstructure.intakeTo(IntakeArmPosition.RETRACTED)
+            m_intake.setIntakeArmPosCmd(IntakeArmPosition.RETRACTED)
         );
 
         // m_driver.y().and(trg_driverOverride).onTrue(m_shooter.turretHomingCmd(false));  //false? im not sure
 
-        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_superstructure.lockTurret());
+        m_driver.povRight().onTrue(m_superstructure.unlockTurret());
 
         // m_driver.start().whileTrue(m_superstructure.activateOuttakeNOSHOOT());
         // trg_optimalPrefireTime.whileTrue(

src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java

@@ -72,7 +72,7 @@ private Command homingCmd() {
     private Command shootWithTimeout(AngularVelocity speed, double seconds) {
         return Commands.sequence(
             tp("shootWithTimeout.START(" + speed + "," + seconds + ")"),
-            m_superstructure.activateOuttakeShotCalc(), //(() -> speed).withTimeout(seconds),
+            m_superstructure.shootWithShotCalcCmd(), //(() -> speed).withTimeout(seconds),
             tp("shootWithTimeout.END")
         );
     }
@@ -126,7 +126,7 @@ public AutoRoutine firstSweep_NoPreload(boolean left) {
         // Homing then intake in parallel with trajectory (no .asProxy() needed)
         routine.active().onTrue(
             homingCmd().andThen(
-                m_superstructure.intake(() -> false).withTimeout(AutonK.kIntakeTimeout)
+                m_superstructure.intakeCmd(() -> false).withTimeout(AutonK.kIntakeTimeout)
             )
         );
 
@@ -153,7 +153,7 @@ public AutoRoutine preloadAuton() {
 
         routine.active().onTrue(
             homingCmd().andThen(
-                m_superstructure.activateOuttakeShotCalc()
+                m_superstructure.shootWithShotCalcCmd()
             )
         );
 
@@ -170,7 +170,7 @@ public AutoRoutine fastOneSweep(boolean left) {
 
         routine.active().onTrue(
             homingCmd().andThen(
-                m_superstructure.intake(() -> false).withTimeout(3.5)
+                m_superstructure.intakeCmd(() -> false).withTimeout(3.5)
             )
         );
 
@@ -196,7 +196,7 @@ public AutoRoutine fastRightTwoSweep_ZigZag() {
 
         routine.active().onTrue(
             homingCmd().andThen(
-                m_superstructure.intake(() -> false).withTimeout(3.5)
+                m_superstructure.intakeCmd(() -> false).withTimeout(3.5)
             )
         );
 
@@ -213,7 +213,7 @@ public AutoRoutine fastRightTwoSweep_ZigZag() {
 
         // Phase 2: intake during zigzag (no final shoot)
         traj2.atTime(2).onTrue(
-            m_superstructure.intake(() -> false).withTimeout(10)
+            m_superstructure.intakeCmd(() -> false).withTimeout(10)
         );
 
         traj2.done().onTrue(m_swerve.xBrakeCmd());
@@ -242,7 +242,7 @@ public AutoRoutine fastTwoSweep_Reshoot(boolean left) {
 
         routine.active().onTrue(
             homingCmd().andThen(
-                m_superstructure.intake(() -> false)
+                m_superstructure.intakeCmd(() -> false)
             ).until(stopIntk1Trg)
         );
 
@@ -261,14 +261,14 @@ public AutoRoutine fastTwoSweep_Reshoot(boolean left) {
         );
 
         traj1.doneFor(AutonK.kShootingTimeout).whileTrue(
-            m_superstructure.activateOuttakeShotCalc()
+            m_superstructure.shootWithShotCalcCmd()
         );
 
         traj1.doneDelayed(AutonK.kShootingTimeout + 0.04).onTrue(traj2.cmd());
 
         // Phase 2: intake during reshoot path
         startIntk2Trg.onTrue(
-            m_superstructure.intake(() -> false) //.until(stopIntk2Trg)
+            m_superstructure.intakeCmd(() -> false) //.until(stopIntk2Trg)
         );
 
         // Final: xBrake + shoot after traj2

src/main/java/frc/robot/subsystems/Intake.java

@@ -121,21 +121,32 @@ public void setIntakeArmPos(Angle rots, double RPSPS) {
         m_intakeArm.setControl(m_MMVReq.withPosition(rots).withAcceleration(RPSPS));
     }
 
-    public boolean isIntakeArmAtPos() {
+    public boolean intakeArmAtTargetPos(double tolerance) {
         var err = m_intakeArm.getClosedLoopError();
         log_intakeArmClosedLoopError.accept(err.getValueAsDouble());
-        boolean isNear = m_intakeArm.getClosedLoopError().isNear(0, 0.01);
-        return isNear;
+        return err.isNear(0, tolerance);
     }
 
-    public Command shimmy() {
+    public boolean intakeArmAtSpecifiedPos(IntakeArmPosition pos, double tolerance) {
+        double err = Math.abs(pos.rots.minus(m_intakeArm.getPosition().getValue()).in(Rotations));
+        return err <= tolerance;
+    }
+
+    public Command retractIntakeArmCmd() {
+        return Commands.sequence(
+            stopIntakeRollers(),
+            setIntakeArmPosCmd(IntakeArmPosition.RETRACTED)
+        );
+    }
+
+    public Command intakeArmShimmy() {
         return Commands.repeatingSequence(
             setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED),
             setIntakeRollersVelocityCmd(0),
             setIntakeArmPosCmd(IntakeArmPosition.SHIMMY),
-            Commands.waitUntil(() -> isIntakeArmAtPos()),
+            Commands.waitUntil(() -> intakeArmAtTargetPos(0.01)),
             setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED),
-            Commands.waitUntil(() -> isIntakeArmAtPos())
+            Commands.waitUntil(() -> intakeArmAtTargetPos(0.01))
         ).finallyDo(() -> setIntakeArmPosCmd(IntakeArmPosition.SAFE));
     }
 
@@ -156,6 +167,10 @@ public Command stopIntakeRollers() {
         return setIntakeRollersVelocityCmd(0);
     }
 
+    public Command barfIntakeRollers() {
+        return barfIntakeRollers();
+    }
+
     public void setIntakeRollersVelocity(double volts) {
         m_intakeRollers.setControl(m_VVReq.withOutput(volts));
     }
@@ -164,6 +179,12 @@ public Command setIntakeRollersVelocityCmd(double volts) {
         return runOnce(() -> setIntakeRollersVelocity(volts));
     }
 
+    public boolean rollersAtMaxSpeed(double tolerance) {
+        var err = m_intakeRollers.getClosedLoopError();
+        log_intakeArmClosedLoopError.accept(err.getValueAsDouble());
+        return err.isNear(0, tolerance);
+    }
+
     public void setIntakeFlapServo(double pos) {
         m_intakeFlapServo.setAngle(pos);