Merge branch 'main' into dcmp-autos

.gitignore

@@ -178,4 +178,7 @@ networktables.json
 src/main/java/org/littletonrobotics/frc2024/BuildConstants.java
 
 # Generated paths
-src/main/deploy/trajectories/*.pathblob
+src/main/deploy/trajectories/*.pathblob
+
+# Shot map temporary file
+~*.xlsx

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

@@ -313,12 +313,7 @@ public RobotContainer() {
                                 FieldConstants.Speaker.centerSpeakerOpening.toTranslation2d()))
                     < Units.feetToMeters(25.0)
                 && rollers.getGamepieceState() == GamepieceState.SHOOTER_STAGED
-                && superstructure.getCurrentGoal() != Superstructure.Goal.PREPARE_CLIMB
-                && superstructure.getCurrentGoal() != Superstructure.Goal.PREPARE_PREPARE_TRAP_CLIMB
-                && superstructure.getCurrentGoal() != Superstructure.Goal.POST_PREPARE_TRAP_CLIMB
-                && superstructure.getCurrentGoal() != Superstructure.Goal.CLIMB
-                && superstructure.getCurrentGoal() != Superstructure.Goal.TRAP
-                && superstructure.getCurrentGoal() != Superstructure.Goal.UNTRAP);
+                && !superstructure.getCurrentGoal().isClimbingGoal());
 
     // Configure autos and buttons
     configureAutos();

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

@@ -11,7 +11,6 @@
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.*;
-import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
@@ -53,31 +52,11 @@ public record AimingParameters(
   @Setter private boolean useAutoLookahead = false;
   private static final double poseBufferSizeSeconds = 2.0;
 
-  /** Arm angle look up table key: meters, values: degrees */
-  private static final InterpolatingDoubleTreeMap armAngleMap = new InterpolatingDoubleTreeMap();
+  private static final double armAngleCoefficient = 57.254371165197;
+  private static final double armAngleExponent = -0.593140189605718;
 
   @AutoLogOutput @Getter @Setter private boolean flywheelAccelerating = false;
-
-  static {
-    armAngleMap.put(1.04, 55.0);
-    armAngleMap.put(1.25, 52.0);
-    armAngleMap.put(1.5, 46.0);
-    armAngleMap.put(1.75, 42.0);
-    armAngleMap.put(2.0, 40.0);
-    armAngleMap.put(2.25, 37.5);
-    armAngleMap.put(2.5, 35.5);
-    armAngleMap.put(2.75, 33.25);
-    armAngleMap.put(2.94, 32.15);
-    armAngleMap.put(3.15, 30.65);
-    armAngleMap.put(3.55, 28.75);
-    armAngleMap.put(3.75, 28.1);
-    armAngleMap.put(4.0, 27.75);
-    armAngleMap.put(4.25, 26.8);
-    armAngleMap.put(4.5, 25.6);
-    armAngleMap.put(8.0, 8.8); // Added in with slope of previous two points to make a best guess
-  }
-
-  @AutoLogOutput @Getter @Setter private double shotCompensationDegrees = -2.0;
+  @AutoLogOutput @Getter @Setter private double shotCompensationDegrees = 0.0;
 
   public void adjustShotCompensationDegrees(double deltaDegrees) {
     shotCompensationDegrees += deltaDegrees;
@@ -260,10 +239,11 @@ public AimingParameters getAimingParameters() {
         robotVelocity.dx * vehicleToGoalDirection.getSin() / targetDistance
             - robotVelocity.dy * vehicleToGoalDirection.getCos() / targetDistance;
 
+    double armAngleDegrees = armAngleCoefficient * Math.pow(targetDistance, armAngleExponent);
     latestParameters =
         new AimingParameters(
             targetVehicleDirection,
-            Rotation2d.fromDegrees(armAngleMap.get(targetDistance) + shotCompensationDegrees),
+            Rotation2d.fromDegrees(armAngleDegrees + shotCompensationDegrees),
             targetDistance,
             feedVelocity);
     return latestParameters;

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

@@ -26,7 +26,7 @@ public final class DriveConstants {
       switch (Constants.getRobot()) {
         case SIMBOT, COMPBOT ->
             DriveConfig.builder()
-                .wheelRadius(Units.inchesToMeters(1.923))
+                .wheelRadius(Units.inchesToMeters(1.9108121863500676))
                 .trackWidthX(Units.inchesToMeters(20.75))
                 .trackWidthY(Units.inchesToMeters(20.75))
                 .bumperWidthX(Units.inchesToMeters(37))

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

@@ -19,7 +19,7 @@ public class FlywheelConstants {
 
   public static final Gains gains =
       switch (Constants.getRobot()) {
-        case COMPBOT -> new Gains(0.18, 0, 0.0006, 0.15, 0.001, 0);
+        case COMPBOT -> new Gains(0.18, 0, 0.0006, 0.38367, 0.00108, 0);
         case DEVBOT -> new Gains(0.0003, 0.0, 0.0, 0.33329, 0.00083, 0.0);
         case SIMBOT -> new Gains(0.05, 0.0, 0.0, 0.01, 0.00103, 0.0);
       };

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

@@ -34,9 +34,9 @@ public class Flywheels extends SubsystemBase {
   private static final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheels/kV", gains.kV());
   private static final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheels/kA", gains.kA());
   private static final LoggedTunableNumber shootingLeftRpm =
-      new LoggedTunableNumber("Flywheels/ShootingLeftRpm", 7000.0);
+      new LoggedTunableNumber("Flywheels/ShootingLeftRpm", 5066.0);
   private static final LoggedTunableNumber shootingRightRpm =
-      new LoggedTunableNumber("Flywheels/ShootingRightRpm", 4500.0);
+      new LoggedTunableNumber("Flywheels/ShootingRightRpm", 7733.0);
   private static final LoggedTunableNumber prepareShootMultiplier =
       new LoggedTunableNumber("Flywheels/PrepareShootMultiplier", 0.75);
   private static final LoggedTunableNumber intakingRpm =

src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java

@@ -64,16 +64,13 @@ public static Leds getInstance() {
   private static final int minLoopCycleCount = 10;
   private static final int length = 12;
   private static final int staticSectionLength = 3;
-  private static final double strobeFastDuration = 0.1;
-  private static final double strobeSlowDuration = 0.2;
+  private static final double strobeDuration = 0.1;
   private static final double breathDuration = 1.0;
   private static final double rainbowCycleLength = 25.0;
   private static final double rainbowDuration = 0.25;
   private static final double waveExponent = 0.4;
   private static final double waveFastCycleLength = 25.0;
   private static final double waveFastDuration = 0.25;
-  private static final double waveSlowCycleLength = 25.0;
-  private static final double waveSlowDuration = 3.0;
   private static final double waveAllianceCycleLength = 15.0;
   private static final double waveAllianceDuration = 2.0;
   private static final double autoFadeTime = 2.5; // 3s nominal
@@ -180,15 +177,15 @@ public synchronized void periodic() {
       }
     } else { // Enabled
       if (requestAmp) {
-        strobe(Color.kWhite, strobeFastDuration);
+        strobe(Color.kWhite, strobeDuration);
       } else if (trapping || climbing || autoDrive || autoShoot) {
         rainbow(rainbowCycleLength, rainbowDuration);
       } else if (hasNote) {
         solid(Color.kGreen);
       }
 
       if (endgameAlert) {
-        strobe(Color.kRed, Color.kGold, strobeFastDuration);
+        strobe(Color.kRed, Color.kGold, strobeDuration);
       }
     }
 

src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/backpack/Backpack.java

@@ -19,7 +19,7 @@
 public class Backpack extends GenericRollerSystem<Backpack.Goal> {
   @RequiredArgsConstructor
   @Getter
-  public enum Goal implements VoltageGoal {
+  public enum Goal implements GenericRollerSystem.VoltageGoal {
     IDLING(() -> 0),
     AMP_SCORING(new LoggedTunableNumber("Backpack/AmpScoringVoltage", 12.0)),
     TRAP_SCORING(new LoggedTunableNumber("Backpack/TrapScoringVoltage", 8.0)),

src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java

@@ -19,7 +19,7 @@
 public class Feeder extends GenericRollerSystem<Feeder.Goal> {
   @RequiredArgsConstructor
   @Getter
-  public enum Goal implements VoltageGoal {
+  public enum Goal implements GenericRollerSystem.VoltageGoal {
     IDLING(() -> 0.0),
     SHUFFLING(new LoggedTunableNumber("Feeder/ShufflingVoltage", 0.0)),
     FLOOR_INTAKING(new LoggedTunableNumber("Feeder/FloorIntakingVoltage", 10.0)),

src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java

@@ -13,6 +13,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import lombok.Getter;
+import lombok.NoArgsConstructor;
+import lombok.RequiredArgsConstructor;
 import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm;
 import org.littletonrobotics.frc2024.subsystems.superstructure.backpackactuator.BackpackActuator;
 import org.littletonrobotics.frc2024.subsystems.superstructure.climber.Climber;
@@ -21,6 +23,9 @@
 
 public class Superstructure extends SubsystemBase {
 
+  @NoArgsConstructor(force = true)
+  @RequiredArgsConstructor
+  @Getter
   public enum Goal {
     STOW,
     BACKPACK_OUT_UNJAM,
@@ -32,15 +37,17 @@ public enum Goal {
     AMP,
     SUBWOOFER,
     PODIUM,
-    RESET_CLIMB,
-    PREPARE_PREPARE_TRAP_CLIMB,
-    PREPARE_CLIMB,
-    POST_PREPARE_TRAP_CLIMB,
-    CLIMB,
-    TRAP,
-    UNTRAP,
+    RESET_CLIMB(true),
+    PREPARE_PREPARE_TRAP_CLIMB(true),
+    PREPARE_CLIMB(true),
+    POST_PREPARE_TRAP_CLIMB(true),
+    CLIMB(true),
+    TRAP(true),
+    UNTRAP(true),
     RESET,
-    DIAGNOSTIC_ARM
+    DIAGNOSTIC_ARM;
+
+    private final boolean climbingGoal;
   }
 
   @Getter private Goal currentGoal = Goal.STOW;
@@ -71,12 +78,7 @@ public void periodic() {
 
     // Retract climber
     if (!climber.retracted()
-        && desiredGoal != Goal.PREPARE_PREPARE_TRAP_CLIMB
-        && desiredGoal != Goal.PREPARE_CLIMB
-        && desiredGoal != Goal.POST_PREPARE_TRAP_CLIMB
-        && desiredGoal != Goal.CLIMB
-        && desiredGoal != Goal.TRAP
-        && desiredGoal != Goal.UNTRAP
+        && !desiredGoal.isClimbingGoal()
         && !DriverStation.isAutonomousEnabled()) {
       currentGoal = Goal.RESET_CLIMB;
     } else {

src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java

@@ -73,7 +73,7 @@ public class Arm {
   @RequiredArgsConstructor
   public enum Goal {
     STOP(() -> 0),
-    FLOOR_INTAKE(new LoggedTunableNumber("Arm/IntakeDegrees", 7.0)),
+    FLOOR_INTAKE(new LoggedTunableNumber("Arm/IntakeDegrees", minAngle.getDegrees())),
     UNJAM_INTAKE(new LoggedTunableNumber("Arm/UnjamDegrees", 40.0)),
     STATION_INTAKE(new LoggedTunableNumber("Arm/StationIntakeDegrees", 45.0)),
     AIM(() -> RobotState.getInstance().getAimingParameters().armAngle().getDegrees()),
@@ -123,6 +123,8 @@ private double getRads() {
   private BooleanSupplier coastSupplier = () -> false;
   private boolean brakeModeEnabled = true;
 
+  private boolean wasNotAuto = false;
+
   public Arm(ArmIO io) {
     this.io = io;
     io.setBrakeMode(true);
@@ -167,12 +169,19 @@ public void periodic() {
         kA);
 
     // Check if disabled
-    if (disableSupplier.getAsBoolean() || goal == Goal.STOP) {
+    // Also run first cycle of auto to reset arm
+    if (disableSupplier.getAsBoolean()
+        || goal == Goal.STOP
+        || (Constants.getMode() == Constants.Mode.SIM
+            && DriverStation.isAutonomousEnabled()
+            && wasNotAuto)) {
       io.stop();
       // Reset profile when disabled
       setpointState = new TrapezoidProfile.State(inputs.positionRads, 0);
     }
     Leds.getInstance().armEstopped = disableSupplier.getAsBoolean() && DriverStation.isEnabled();
+    // Track autonomous enabled
+    wasNotAuto = !DriverStation.isAutonomousEnabled();
 
     // Set coast mode with override
     setBrakeMode(!coastSupplier.getAsBoolean());

src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java

@@ -20,7 +20,7 @@ public class ArmConstants {
   public static final Translation2d armOrigin = new Translation2d(-0.238, 0.298);
   public static final Rotation2d minAngle =
       switch (Constants.getRobot()) {
-        default -> Rotation2d.fromDegrees(6.85); // Measured from hardstop 3/12/24
+        default -> Rotation2d.fromDegrees(5.8); // Measured from hardstop 3/31/24
         case DEVBOT -> Rotation2d.fromDegrees(10.0);
       };
   public static final Rotation2d maxAngle = Rotation2d.fromDegrees(110.0);
@@ -46,8 +46,7 @@ public class ArmConstants {
   /** The offset of the arm encoder in radians. */
   public static final double armEncoderOffsetRads =
       switch (Constants.getRobot()) {
-        default ->
-            1.1980389953386859 + .006; // 1.0753205323078345 rad as measured at hardstop on 3/12/24,
+        default -> 1.1886991875;
           // corresponding to an arm position of 0.11965050145508001 rad
         case DEVBOT -> -1.233 - Math.PI / 2.0;
       };
@@ -62,7 +61,7 @@ public class ArmConstants {
       switch (Constants.getRobot()) {
         case SIMBOT -> new Gains(90.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
         case DEVBOT -> new Gains(75.0, 0.0, 2.5, 0.0, 0.0, 0.0, 0.0);
-        case COMPBOT -> new Gains(6000.0, 0.0, 250.0, 8.4, 0.0, 0.0, 22.9);
+        case COMPBOT -> new Gains(7000.0, 0.0, 250.0, 8.4, 0.0, 0.0, 22.9);
       };
 
   public static TrapezoidProfile.Constraints profileConstraints =

src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java

@@ -43,9 +43,6 @@ default void setBrakeMode(boolean enabled) {}
   /** Set PID values */
   default void setPID(double p, double i, double d) {}
 
-  /** Sets position of internal encoder */
-  default void setPosition(double positionRads) {}
-
   /** Stops motors */
   default void stop() {}
 }

src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java

@@ -22,6 +22,7 @@ public class ArmIOKrakenFOC implements ArmIO {
   // Hardware
   private final TalonFX leaderTalon;
   private final TalonFX followerTalon;
+  private final CANcoder absoluteEncoder;
 
   // Status Signals
   private final StatusSignal<Double> internalPositionRotations;
@@ -47,7 +48,7 @@ public ArmIOKrakenFOC() {
     leaderTalon = new TalonFX(ArmConstants.leaderID, "*");
     followerTalon = new TalonFX(ArmConstants.followerID, "*");
     followerTalon.setControl(new Follower(ArmConstants.leaderID, true));
-    CANcoder absoluteEncoder = new CANcoder(ArmConstants.armEncoderID, "*");
+    absoluteEncoder = new CANcoder(ArmConstants.armEncoderID, "*");
 
     // Arm Encoder Configs
     CANcoderConfiguration armEncoderConfig = new CANcoderConfiguration();
@@ -177,12 +178,6 @@ public void setPID(double p, double i, double d) {
     leaderTalon.getConfigurator().apply(config, 0.01);
   }
 
-  @Override
-  public void setPosition(double positionRads) {
-    //    leaderTalon.setPosition(Units.radiansToRotations(positionRads * reduction), 0.2); TODO:
-    // Figure this out.
-  }
-
   @Override
   public void stop() {
     leaderTalon.setControl(new NeutralOut());

src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java

@@ -18,6 +18,8 @@
 import org.littletonrobotics.frc2024.Constants;
 
 public class ArmIOSim implements ArmIO {
+  private static final double autoStartAngle = Units.degreesToRadians(80.0);
+
   private final SingleJointedArmSim sim =
       new SingleJointedArmSim(
           DCMotor.getKrakenX60Foc(2),
@@ -35,7 +37,6 @@ public class ArmIOSim implements ArmIO {
 
   private boolean controllerNeedsReset = false;
   private boolean closedLoop = true;
-
   private boolean wasNotAuto = true;
 
   public ArmIOSim() {
@@ -49,14 +50,13 @@ public void updateInputs(ArmIOInputs inputs) {
     if (DriverStation.isDisabled()) {
       controllerNeedsReset = true;
     }
-    // Assume starting at ~80 degrees
+
+    // Reset at start of auto
     if (wasNotAuto && DriverStation.isAutonomousEnabled()) {
-      sim.setState(Units.degreesToRadians(80.0), 0.0);
+      sim.setState(autoStartAngle, 0.0);
       wasNotAuto = false;
     }
-    if (!DriverStation.isAutonomousEnabled()) {
-      wasNotAuto = true;
-    }
+    wasNotAuto = !DriverStation.isAutonomousEnabled();
 
     sim.update(Constants.loopPeriodSecs);
 
@@ -102,8 +102,7 @@ public void stop() {
     sim.setInputVoltage(appliedVoltage);
   }
 
-  @Override
-  public void setPosition(double position) {
+  private void setPosition(double position) {
     positionOffset = position - sim.getAngleRads();
   }
 }

src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/backpackactuator/BackpackActuator.java

@@ -19,7 +19,7 @@
 public class BackpackActuator extends GenericSlamElevator<BackpackActuator.Goal> {
   @RequiredArgsConstructor
   @Getter
-  public enum Goal implements SlamElevatorGoal {
+  public enum Goal implements GenericSlamElevator.SlamElevatorGoal {
     RETRACT(
         new LoggedTunableNumber("BackpackActuator/RetractingCurrent", -25.0),
         true,