Feature disable motion magic

ascope_assets/Robot_MA24B/config.json

@@ -0,0 +1,85 @@
+{
+  "name": "MA24B",
+  "rotations": [
+    {
+      "axis": "x",
+      "degrees": 90
+    },
+    {
+      "axis": "z",
+      "degrees": 90
+    }
+  ],
+  "position": [
+    0,
+    0,
+    0
+  ],
+  "cameras": [
+    {
+      "name": "Northstar 0",
+      "rotations": [
+        {
+          "axis": "y",
+          "degrees": -28.125
+        },
+        {
+          "axis": "z",
+          "degrees": 30.0
+        }
+      ],
+      "position": [
+        0.247269,
+        0.24729186,
+        0.2244598
+      ],
+      "resolution": [
+        1600,
+        1200
+      ],
+      "fov": 75
+    },
+    {
+      "name": "Northstar 1",
+      "rotations": [
+        {
+          "axis": "y",
+          "degrees": -28.125
+        },
+        {
+          "axis": "z",
+          "degrees": -30.0
+        }
+      ],
+      "position": [
+        0.247269,
+        -0.24729186,
+        0.2244598
+      ],
+      "resolution": [
+        1600,
+        1200
+      ],
+      "fov": 75
+    }
+  ],
+  "components": [
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 90
+        },
+        {
+          "axis": "z",
+          "degrees": 90
+        }
+      ],
+      "zeroedPosition": [
+        0.238,
+        0.0,
+        -0.298
+      ]
+    }
+  ]
+}

src/main/java/org/littletonrobotics/frc2024/RobotContainer.java

@@ -35,12 +35,15 @@
 import org.littletonrobotics.frc2024.subsystems.rollers.feeder.Feeder;
 import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIO;
 import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOKrakenFOC;
+import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOSim;
 import org.littletonrobotics.frc2024.subsystems.rollers.indexer.Indexer;
 import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIO;
+import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSim;
 import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSparkFlex;
 import org.littletonrobotics.frc2024.subsystems.rollers.intake.Intake;
 import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIO;
 import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIOKrakenFOC;
+import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIOSim;
 import org.littletonrobotics.frc2024.subsystems.superstructure.Superstructure;
 import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm;
 import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIO;
@@ -121,11 +124,18 @@ public RobotContainer() {
                   new ModuleIOSim(DriveConstants.moduleConfigs[2]),
                   new ModuleIOSim(DriveConstants.moduleConfigs[3]));
           flywheels = new Flywheels(new FlywheelsIOSim());
+
+          feeder = new Feeder(new FeederIOSim());
+          indexer = new Indexer(new IndexerIOSim());
+          intake = new Intake(new IntakeIOSim());
+          rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIO() {});
+
           arm = new Arm(new ArmIOSim());
         }
       }
     }
 
+    // No-op implementation for replay
     if (drive == null) {
       drive =
           new Drive(
@@ -200,20 +210,20 @@ private void configureButtonBindings() {
                 () ->
                     drive.setTeleopDriveGoal(
                         -controller.getLeftY(), -controller.getLeftX(), -controller.getRightX()))
-            .withName("DriveTeleop"));
+            .withName("Drive Teleop Input"));
+
+    // Aim and Rev Flywheels
     controller
         .a()
         .whileTrue(
             Commands.startEnd(drive::setAutoAimGoal, drive::clearAutoAimGoal)
-                .alongWith(superstructure.aimCommand(), flywheels.shootCommand()));
-
-    Trigger readyToShootTrigger =
+                .alongWith(superstructure.aim(), flywheels.shoot())
+                .withName("Aim"));
+    // Shoot
+    Trigger readyToShoot =
         new Trigger(
-            () ->
-                drive.isAutoAimGoalCompleted()
-                    && flywheels.atSetpoint()
-                    && superstructure.atArmSetpoint());
-    readyToShootTrigger
+            () -> drive.isAutoAimGoalCompleted() && superstructure.atGoal() && flywheels.atGoal());
+    readyToShoot
         .whileTrue(
             Commands.run(
                 () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0)))
@@ -222,26 +232,29 @@ private void configureButtonBindings() {
                 () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0)));
     controller
         .rightTrigger()
-        .and(readyToShootTrigger)
+        .and(readyToShoot)
         .onTrue(
-            rollers
-                .feedShooterCommand()
-                .withTimeout(1.0)
-                // Take over superstructure and flywheels, cancelling the main aiming command
-                .deadlineWith(superstructure.aimCommand(), flywheels.shootCommand()));
-
+            Commands.parallel(
+                    Commands.waitSeconds(0.5),
+                    Commands.waitUntil(controller.rightTrigger().negate()))
+                .deadlineWith(rollers.feedShooter(), superstructure.aim(), flywheels.shoot()));
+    // Intake Floor
     controller
         .leftTrigger()
         .whileTrue(
             superstructure
-                .floorIntakeCommand()
-                .alongWith(Commands.waitSeconds(0.25).andThen(rollers.floorIntakeCommand())));
+                .intake()
+                .alongWith(
+                    Commands.waitUntil(superstructure::atGoal).andThen(rollers.floorIntake()))
+                .withName("Floor Intake"));
+    // Eject Floor
     controller
         .leftBumper()
         .whileTrue(
             superstructure
-                .floorIntakeCommand()
-                .alongWith(Commands.waitSeconds(0.25).andThen(rollers.ejectFloorCommand())));
+                .intake()
+                .alongWith(Commands.waitUntil(superstructure::atGoal).andThen(rollers.ejectFloor()))
+                .withName("Eject To Floor"));
 
     controller
         .y()

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java

@@ -16,7 +16,6 @@
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 
 public class ModuleIOSim implements ModuleIO {
-
   private final DCMotorSim driveSim =
       new DCMotorSim(DCMotor.getKrakenX60Foc(1), moduleConstants.driveReduction(), 0.025);
   private final DCMotorSim turnSim =

src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java

@@ -24,7 +24,7 @@ public class FlywheelConstants {
         case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.09078, 0.00103, 0.0);
       };
 
-  public record Config(int leftID, int rightID, double reduction, double toleranceRPM) {}
+  public record Config(int leftID, int rightID, double reduction, double toleranceRpm) {}
 
   public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {}
 }

src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java

@@ -26,31 +26,30 @@ public class Flywheels extends SubsystemBase {
   private static final LoggedTunableNumber kS = new LoggedTunableNumber("Flywheels/kS", gains.kS());
   private static final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheels/kV", gains.kV());
   private static final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheels/kA", gains.kA());
-  private static LoggedTunableNumber shootingLeftRPM =
-      new LoggedTunableNumber("Superstructure/ShootingLeftRPM", 6000.0);
-  private static LoggedTunableNumber shootingRightRPM =
-      new LoggedTunableNumber("Superstructure/ShootingRightRPM", 4000.0);
-  private static LoggedTunableNumber idleLeftRPM =
-      new LoggedTunableNumber("Superstructure/IdleLeftRPM", 200.0);
-  private static LoggedTunableNumber idleRightRPM =
-      new LoggedTunableNumber("Superstructure/IdleRightRPM", 200.0);
-
-  private static LoggedTunableNumber intakingLeftRPM =
-      new LoggedTunableNumber("Superstructure/IntakingLeftRPM", -2000.0);
-  private static LoggedTunableNumber intakingRightRPM =
-      new LoggedTunableNumber("Superstructure/IntakingRightRPM", -2000.0);
+  private static final LoggedTunableNumber shootingLeftRpm =
+      new LoggedTunableNumber("Superstructure/ShootingLeftRpm", 6000.0);
+  private static final LoggedTunableNumber shootingRightRpm =
+      new LoggedTunableNumber("Superstructure/ShootingRightRpm", 4000.0);
+  private static final LoggedTunableNumber idleLeftRpm =
+      new LoggedTunableNumber("Superstructure/IdleLeftRpm", 1500.0);
+  private static final LoggedTunableNumber idleRightRpm =
+      new LoggedTunableNumber("Superstructure/IdleRightRpm", 1000.0);
+  private static final LoggedTunableNumber intakingRpm =
+      new LoggedTunableNumber("Superstructure/IntakingRpm", -2000.0);
+  private static final LoggedTunableNumber ejectingRpm =
+      new LoggedTunableNumber("Superstructure/EjectingRpm", 2000.0);
   private static final LoggedTunableNumber shooterTolerance =
-      new LoggedTunableNumber("Flywheels/ToleranceRPM", config.toleranceRPM());
+      new LoggedTunableNumber("Flywheels/ToleranceRpm", config.toleranceRpm());
 
   private final FlywheelsIO io;
   private final FlywheelsIOInputsAutoLogged inputs = new FlywheelsIOInputsAutoLogged();
 
   @RequiredArgsConstructor
   public enum Goal {
-    STOP(() -> 0.0, () -> 0.0),
-    IDLE(idleLeftRPM, idleRightRPM),
-    SHOOTING(shootingLeftRPM, shootingRightRPM),
-    INTAKING(intakingLeftRPM, intakingRightRPM),
+    IDLE(idleLeftRpm, idleRightRpm),
+    SHOOT(shootingLeftRpm, shootingRightRpm),
+    INTAKE(intakingRpm, intakingRpm),
+    EJECT(ejectingRpm, ejectingRpm),
     CHARACTERIZING(() -> 0.0, () -> 0.0);
 
     private final DoubleSupplier leftSetpoint;
@@ -69,7 +68,8 @@ private double getRightSetpoint() {
 
   public Flywheels(FlywheelsIO io) {
     this.io = io;
-    setDefaultCommand(runOnce(() -> goal = Goal.IDLE).withName("FlywheelsIdle"));
+
+    setDefaultCommand(runOnce(this::goIdle).withName("Flywheels Idle"));
   }
 
   @Override
@@ -83,20 +83,18 @@ public void periodic() {
         hashCode(), kSVA -> io.setFF(kSVA[0], kSVA[1], kSVA[2]), kS, kV, kA);
 
     if (DriverStation.isDisabled()) {
-      goal = Goal.STOP;
+      io.stop();
     }
 
-    switch (goal) {
-      case STOP -> io.stop();
-      case CHARACTERIZING -> {} // Handled by runCharacterizationVolts
-      default -> io.runVelocity(goal.getLeftSetpoint(), goal.getRightSetpoint());
+    if (goal != Goal.CHARACTERIZING) {
+      io.runVelocity(goal.getLeftSetpoint(), goal.getRightSetpoint());
     }
 
     Logger.recordOutput("Flywheels/Goal", goal);
-    Logger.recordOutput("Flywheels/LeftSetpointRPM", goal.getLeftSetpoint());
-    Logger.recordOutput("Flywheels/RightSetpointRPM", goal.getRightSetpoint());
-    Logger.recordOutput("Flywheels/LeftRPM", inputs.leftVelocityRpm);
-    Logger.recordOutput("Flywheels/RightRPM", inputs.rightVelocityRpm);
+    Logger.recordOutput("Flywheels/LeftSetpointRpm", goal.getLeftSetpoint());
+    Logger.recordOutput("Flywheels/RightSetpointRpm", goal.getRightSetpoint());
+    Logger.recordOutput("Flywheels/LeftRpm", inputs.leftVelocityRpm);
+    Logger.recordOutput("Flywheels/RightRpm", inputs.rightVelocityRpm);
   }
 
   public void runCharacterizationVolts(double volts) {
@@ -109,19 +107,25 @@ public double getCharacterizationVelocity() {
     return (inputs.leftVelocityRpm + inputs.rightVelocityRpm) / 2.0;
   }
 
-  @AutoLogOutput(key = "Shooter/AtSetpoint")
-  public boolean atSetpoint() {
-    return Math.abs(inputs.leftVelocityRpm - goal.leftSetpoint.getAsDouble())
-            <= shooterTolerance.get()
-        && Math.abs(inputs.rightVelocityRpm - goal.rightSetpoint.getAsDouble())
-            <= shooterTolerance.get();
+  @AutoLogOutput(key = "Flywheels/AtGoal")
+  public boolean atGoal() {
+    return Math.abs(inputs.leftVelocityRpm - goal.getLeftSetpoint()) <= shooterTolerance.get()
+        && Math.abs(inputs.rightVelocityRpm - goal.getRightSetpoint()) <= shooterTolerance.get();
+  }
+
+  private void goIdle() {
+    goal = Goal.IDLE;
+  }
+
+  public Command shoot() {
+    return startEnd(() -> goal = Goal.SHOOT, this::goIdle).withName("Flywheels Shooting");
   }
 
-  public Command shootCommand() {
-    return startEnd(() -> goal = Goal.SHOOTING, () -> goal = Goal.IDLE).withName("FlywheelsShoot");
+  public Command intake() {
+    return startEnd(() -> goal = Goal.INTAKE, this::goIdle).withName("Flywheels Intaking");
   }
 
-  public Command intakeCommand() {
-    return startEnd(() -> goal = Goal.INTAKING, () -> goal = Goal.IDLE).withName("FlywheelsIntake");
+  public Command eject() {
+    return startEnd(() -> goal = Goal.EJECT, this::goIdle).withName("Flywheels Ejecting");
   }
 }

src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystem.java

@@ -29,6 +29,6 @@ public void periodic() {
     Logger.processInputs(name, inputs);
 
     io.runVolts(getGoal().getVoltageSupplier().getAsDouble());
-    Logger.recordOutput(name + "/Goal", getGoal().toString());
+    Logger.recordOutput("Rollers/" + name + "Goal", getGoal().toString());
   }
 }

src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIO.java

@@ -16,6 +16,7 @@ abstract class GenericRollerSystemIOInputs {
     public double velocityRadsPerSec = 0.0;
     public double appliedVoltage = 0.0;
     public double outputCurrent = 0.0;
+    public double tempCelsius = 0.0;
   }
 
   default void updateInputs(GenericRollerSystemIOInputs inputs) {}

src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOKrakenFOC.java

@@ -25,6 +25,7 @@ public abstract class GenericRollerSystemIOKrakenFOC implements GenericRollerSys
   private final StatusSignal<Double> velocity;
   private final StatusSignal<Double> appliedVoltage;
   private final StatusSignal<Double> outputCurrent;
+  private final StatusSignal<Double> tempCelsius;
 
   // Single shot for voltage mode, robot loop will call continuously
   private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0);
@@ -49,9 +50,10 @@ public GenericRollerSystemIOKrakenFOC(
     velocity = motor.getVelocity();
     appliedVoltage = motor.getMotorVoltage();
     outputCurrent = motor.getTorqueCurrent();
+    tempCelsius = motor.getDeviceTemp();
 
     BaseStatusSignal.setUpdateFrequencyForAll(
-        50.0, position, velocity, appliedVoltage, outputCurrent);
+        50.0, position, velocity, appliedVoltage, outputCurrent, tempCelsius);
   }
 
   @Override
@@ -62,6 +64,7 @@ public void updateInputs(GenericRollerSystemIOInputs inputs) {
     inputs.velocityRadsPerSec = Units.rotationsToRadians(velocity.getValueAsDouble()) / reduction;
     inputs.appliedVoltage = appliedVoltage.getValueAsDouble();
     inputs.outputCurrent = outputCurrent.getValueAsDouble();
+    inputs.tempCelsius = tempCelsius.getValueAsDouble();
   }
 
   @Override