Feature disable motion magic

build.gradle

@@ -112,7 +112,7 @@ task(generateDriveTrajectories, dependsOn: "classes", type: JavaExec) {
     mainClass = "org.littletonrobotics.frc2024.subsystems.drive.trajectory.GenerateTrajectories"
     classpath = sourceSets.main.runtimeClasspath
 }
-compileJava.finalizedBy generateDriveTrajectories
+//compileJava.finalizedBy generateDriveTrajectories
 
 // Check robot type when deploying
 task(checkRobot, dependsOn: "classes", type: JavaExec) {

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

@@ -21,7 +21,7 @@
  */
 public final class Constants {
   public static final int loopPeriodMs = 20;
-  private static RobotType robotType = RobotType.DEVBOT;
+  private static RobotType robotType = RobotType.SIMBOT;
   public static final boolean tuningMode = true;
 
   public static RobotType getRobot() {

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

@@ -19,7 +19,6 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import org.littletonrobotics.frc2024.commands.FeedForwardCharacterization;
-import org.littletonrobotics.frc2024.commands.auto.AutoCommands;
 import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVision;
 import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionConstants;
 import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO;
@@ -35,12 +34,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 +123,19 @@ 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());
+          superstructure = new Superstructure(arm);
         }
       }
     }
 
+    // No-op implementation for replay
     if (drive == null) {
       drive =
           new Drive(
@@ -170,8 +180,8 @@ public RobotContainer() {
 
   private void configureAutos() {
     autoChooser.addDefaultOption("Do Nothing", Commands.none());
-    AutoCommands autoCommands = new AutoCommands(drive, superstructure);
-    autoChooser.addOption("Drive Straight", autoCommands.driveStraight());
+    //    AutoCommands autoCommands = new AutoCommands(drive, superstructure);
+    //    autoChooser.addOption("Drive Straight", autoCommands.driveStraight());
 
     // Set up feedforward characterization
     autoChooser.addOption(
@@ -200,20 +210,23 @@ 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
+                    && superstructure.atShootingSetpoint()
+                    && flywheels.atGoal());
+    readyToShoot
         .whileTrue(
             Commands.run(
                 () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0)))
@@ -222,26 +235,24 @@ private void configureButtonBindings() {
                 () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0)));
     controller
         .rightTrigger()
-        .and(readyToShootTrigger)
-        .onTrue(
-            rollers
-                .feedShooterCommand()
-                .withTimeout(1.0)
-                // Take over superstructure and flywheels, cancelling the main aiming command
-                .deadlineWith(superstructure.aimCommand(), flywheels.shootCommand()));
-
+        .and(readyToShoot)
+        .onTrue(rollers.feedShooter().withTimeout(1.0).withName("Shoot"));
+    // Intake Floor
     controller
         .leftTrigger()
         .whileTrue(
             superstructure
-                .floorIntakeCommand()
-                .alongWith(Commands.waitSeconds(0.25).andThen(rollers.floorIntakeCommand())));
+                .intake()
+                .alongWith(rollers.floorIntake().onlyIf(superstructure::atGoal))
+                .withName("Floor Intake"));
+    // Eject Floor
     controller
         .leftBumper()
         .whileTrue(
             superstructure
-                .floorIntakeCommand()
-                .alongWith(Commands.waitSeconds(0.25).andThen(rollers.ejectFloorCommand())));
+                .intake()
+                .alongWith(rollers.ejectFloor().onlyIf(superstructure::atGoal))
+                .withName("Eject Floor"));
 
     controller
         .y()

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

@@ -15,6 +15,7 @@
 import java.util.function.DoubleSupplier;
 import lombok.Getter;
 import lombok.RequiredArgsConstructor;
+import lombok.Setter;
 import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -26,31 +27,31 @@ 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),
+    IDLING(idleLeftRpm, idleRightRpm),
+    SHOOTING(shootingLeftRpm, shootingRightRpm),
+    INTAKING(intakingRpm, intakingRpm),
+    EJECTING(ejectingRpm, ejectingRpm),
     CHARACTERIZING(() -> 0.0, () -> 0.0);
 
     private final DoubleSupplier leftSetpoint;
@@ -65,11 +66,15 @@ private double getRightSetpoint() {
     }
   }
 
-  @Getter private Goal goal = Goal.IDLE;
+  @Getter @Setter private Goal goal = Goal.IDLING;
+  private double characterizationVolts = 0.0;
+  private boolean characterizing = false;
 
   public Flywheels(FlywheelsIO io) {
+    System.out.println("[Init] Creating Shooter");
     this.io = io;
-    setDefaultCommand(runOnce(() -> goal = Goal.IDLE).withName("FlywheelsIdle"));
+
+    setDefaultCommand(idle());
   }
 
   @Override
@@ -83,7 +88,7 @@ public void periodic() {
         hashCode(), kSVA -> io.setFF(kSVA[0], kSVA[1], kSVA[2]), kS, kV, kA);
 
     if (DriverStation.isDisabled()) {
-      goal = Goal.STOP;
+      setGoal(Goal.STOP);
     }
 
     switch (goal) {
@@ -93,14 +98,14 @@ public void periodic() {
     }
 
     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) {
-    goal = Goal.CHARACTERIZING;
+    setGoal(Goal.CHARACTERIZING);
     io.runCharacterizationLeftVolts(volts);
     io.runCharacterizationRightVolts(volts);
   }
@@ -109,19 +114,32 @@ 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();
+  }
+
+  public Command stop() {
+    return runOnce(() -> setGoal(Goal.STOP)).withName("Flywheels Stop");
+  }
+
+  public Command idle() {
+    return runOnce(() -> setGoal(Goal.IDLING)).withName("Flywheels Idle");
+  }
+
+  public Command shoot() {
+    return startEnd(() -> setGoal(Goal.SHOOTING), () -> setGoal(Goal.IDLING))
+        .withName("Flywheels Shooting");
   }
 
-  public Command shootCommand() {
-    return startEnd(() -> goal = Goal.SHOOTING, () -> goal = Goal.IDLE).withName("FlywheelsShoot");
+  public Command intake() {
+    return startEnd(() -> setGoal(Goal.INTAKING), () -> setGoal(Goal.IDLING))
+        .withName("Flywheels Intaking");
   }
 
-  public Command intakeCommand() {
-    return startEnd(() -> goal = Goal.INTAKING, () -> goal = Goal.IDLE).withName("FlywheelsIntake");
+  public Command eject() {
+    return startEnd(() -> setGoal(Goal.EJECTING), () -> setGoal(Goal.IDLING))
+        .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

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

@@ -0,0 +1,46 @@
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package org.littletonrobotics.frc2024.subsystems.rollers;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+
+public class GenericRollerSystemIOSim implements GenericRollerSystemIO {
+  private final FlywheelSim sim;
+  private double appliedVoltage = 0.0;
+
+  public GenericRollerSystemIOSim(DCMotor motorModel, double reduction, double moi) {
+    sim = new FlywheelSim(motorModel, reduction, moi);
+  }
+
+  @Override
+  public void updateInputs(GenericRollerSystemIOInputs inputs) {
+    if (DriverStation.isDisabled()) {
+      runVolts(0.0);
+    }
+
+    sim.update(0.02);
+    inputs.positionRads += sim.getAngularVelocityRadPerSec() * 0.02;
+    inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec();
+    inputs.appliedVoltage = appliedVoltage;
+    inputs.outputCurrent = sim.getCurrentDrawAmps();
+  }
+
+  @Override
+  public void runVolts(double volts) {
+    appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0);
+    sim.setInputVoltage(appliedVoltage);
+  }
+
+  @Override
+  public void stop() {
+    runVolts(0.0);
+  }
+}