Merge changes from IRI

.github/workflows/gradle.yml

@@ -6,9 +6,9 @@ name: CI
 # events but only for the main branch.
 on:
   push:
-    branches: [ main, Ferndale, District-2, States, Worlds ]
+    branches: [ main, Ferndale, District-2, States, Worlds, iri ]
   pull_request:
-    branches: [ main, Ferndale, District-2, States, Worlds ]
+    branches: [ main, Ferndale, District-2, States, Worlds, iri ]
 
 # A workflow run is made up of one or more jobs that can run sequentially or in parallel
 jobs:

ThriftyTest/simgui.json

@@ -54,6 +54,16 @@
           "visible": true
         }
       },
+      "/SmartDashboard/Prep/Set Left": {
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/Prep/Set Right": {
+        "window": {
+          "visible": true
+        }
+      },
       "/SmartDashboard/Super Field": {
         "Algae": {
           "image": "../resources/algae.jpg",
@@ -225,6 +235,9 @@
       }
     }
   },
+  "NetworkTables Info": {
+    "visible": true
+  },
   "Plot": {
     "Plot <0>": {
       "plots": [

ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto

@@ -29,7 +29,7 @@
               {
                 "type": "named",
                 "data": {
-                  "name": "Upper Algae"
+                  "name": "Lower Algae"
                 }
               }
             ]

ThriftyTest/src/main/java/frc/robot/Constants.java

@@ -44,8 +44,10 @@ public static class FieldConstants {
     public static final Pose2d kBargeFromLeft = new Pose2d(7.5, 6.6, Rotation2d.fromDegrees(21));
 
     public static final Pose2d kStartCenter = new Pose2d(7.076, 3.991, Rotation2d.kPi);
-    public static final Pose2d kStartRight = new Pose2d(7.076, 1.883, Rotation2d.kPi);
-    public static final Pose2d kStartLeft = new Pose2d(7.076, 6.027, Rotation2d.kPi);
+    public static final Pose2d kStartRight = new Pose2d(7.076, 1.2, Rotation2d.kPi);
+    public static final Pose2d kStartLeft = new Pose2d(7.076, 6.6, Rotation2d.kPi);
+
+    public static final Rotation2d kStartHeading = Rotation2d.kPi; // facing our alliance
   }
 
   public static final class StateSpaceConstants {

ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java

@@ -6,6 +6,7 @@
 import frc.robot.superstructure.EnterableState;
 import frc.robot.superstructure.Superstructure;
 import frc.robot.superstructure.Superstructure.Subsystems;
+import frc.robot.superstructure.states.SeedAngle;
 import frc.robot.superstructure.states.SeedPose;
 import frc.robot.superstructure.states.Stow;
 import frc.robot.superstructure.states.TrackAlgae;
@@ -15,6 +16,7 @@ public void bind(Superstructure superstructure) {
     SmartDashboard.putData("Prep/Set Center", superstructure.enter(SeedPose.center()));
     SmartDashboard.putData("Prep/Set Left", superstructure.enter(SeedPose.left()));
     SmartDashboard.putData("Prep/Set Right", superstructure.enter(SeedPose.right()));
+    SmartDashboard.putData("Prep/Set Angle", superstructure.enter(SeedAngle.reverse()));
 
     SmartDashboard.putData("Test/Stow", superstructure.enter(new Stow()));
     SmartDashboard.putData("Test/Elevator Up", superstructure.enter(new EnterableState() {

ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java

@@ -35,8 +35,8 @@ public void bind(Superstructure superstructure) {
     superstructure.setDrive(superstructure.enterWithoutProxy(new TeleopDrive(m_x, m_y, m_rot)));
 
     m_resetHeading.onTrue(superstructure.enter(new HeadingReset()));
-    m_smartAlign.and(superstructure.holdingAlgae()).whileTrue(superstructure.enter(new Align(
-        new APTarget(FieldConstants.k_processor).withEntryAngle(Rotation2d.kCW_Pi_2))));
+    // m_smartAlign.and(superstructure.holdingAlgae()).whileTrue(superstructure.enter(new Align(
+    //     new APTarget(FieldConstants.k_processor).withEntryAngle(Rotation2d.kCW_Pi_2))));
     m_smartAlign.and(superstructure.holdingAlgae().negate()).whileTrue(superstructure.enter(
         new DeferredAlign(AlignLocation.Center)));
     m_leftAlign.whileTrue(superstructure.enter(new DeferredAlign(AlignLocation.Left)));

ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java

@@ -1,8 +1,11 @@
 package frc.robot.subsystems.algae;
 
+import static edu.wpi.first.units.Units.Seconds;
+
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 
+import frc.robot.utils.SuperDebouncer.DebounceType;
 import edu.wpi.first.math.filter.MedianFilter;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -12,18 +15,23 @@
 import frc.robot.subsystems.PassiveSubsystem;
 import frc.robot.subsystems.algae.AlgaeIO.AlgaeIOInputs;
 import frc.robot.utils.LoopTimer;
+import frc.robot.utils.OnboardLogger;
+import frc.robot.utils.SuperDebouncer;
 
 public class Algae extends PassiveSubsystem {
   @SuppressWarnings("unused")
   private final Logger m_logger = LoggerFactory.getLogger(Algae.class);
 
+  private final OnboardLogger m_ologger;
+
   private final LoopTimer m_timer;
 
   private final AlgaeIO m_io;
   private AlgaeIOInputs m_inputs;
   private AlgaeIOInputsLogger m_inputsLogger;
 
   private boolean m_hasAlgae;
+  private SuperDebouncer m_debouncer = new SuperDebouncer(AlgaeConstants.kAlgaeDebounceTime.in(Seconds), DebounceType.kFalling);
 
   private MedianFilter m_filter = new MedianFilter(10);
 
@@ -34,8 +42,12 @@ public Algae() {
     } else {
       m_io = new AlgaeIOSim();
     }
+    resetDebouncer();
     m_inputs = new AlgaeIOInputs();
     m_inputsLogger = new AlgaeIOInputsLogger(m_inputs);
+    m_ologger = new OnboardLogger("Algae");
+    m_ologger.registerBoolean("Holding", holdingAlgae());
+
     RobotObserver.setAlgaePieceHeldSupplier(this.holdingAlgae());
     m_timer = new LoopTimer("Algae");
   }
@@ -74,7 +86,8 @@ public void periodic() {
     m_timer.reset();
     m_io.updateInputs(m_inputs);
     m_inputsLogger.log();
-    m_hasAlgae = getTorqueCurrent() >= AlgaeConstants.kTorqueCurrentThreshold;
+    m_hasAlgae = m_debouncer.calculate(getTorqueCurrent() >= AlgaeConstants.kTorqueCurrentThreshold);
+    m_ologger.log();
     m_timer.log();
   }
 
@@ -94,6 +107,10 @@ public Command intake() {
         .unless(holdingAlgae());
   }
 
+  private void resetDebouncer() {
+    m_debouncer.overrideTimer();
+  }
+
   /**
    * Ejects an algae with the correct conditions for a net score
    */
@@ -103,6 +120,7 @@ public Command net() {
         Commands.waitSeconds(AlgaeConstants.kNetScoreTime))
 
         .finallyDo(this::keep)
+        .finallyDo(this::resetDebouncer)
         .onlyIf(holdingAlgae());
   }
 

ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java

@@ -1,21 +1,28 @@
 package frc.robot.subsystems.algae;
 
+import static edu.wpi.first.units.Units.Seconds;
+
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
+import edu.wpi.first.units.measure.Time;
+
 public final class AlgaeConstants {
   protected static final int kMotorID = 60;
 
   protected static final double kIntakeVoltage = 12;
   protected static final double kNetEjectVoltage = -3.0;
   protected static final double kProcessorEjectVoltage = -3.2;
-  protected static final double kHoldVoltage = 2.7;
+  protected static final double kHoldVoltage = 6.0;
+
+  protected static final double kTorqueCurrentThreshold = 75; // We should consider 40-55 range as well.
+  protected static final Time kAlgaeDebounceTime = Seconds.of(0.8);
 
-  protected static final double kTorqueCurrentThreshold = 75;
   protected static final double kSupplyCurrentLimit = 40.0;
+  protected static final double kStatorCurrentLimit = 125.0;
 
   protected static final double kProcessorScoreTime = 2.0;
   protected static final double kNetScoreTime = 0.4;
@@ -27,6 +34,8 @@ public final class AlgaeConstants {
 
       .withCurrentLimits(new CurrentLimitsConfigs()
           .withSupplyCurrentLimitEnable(true)
-          .withSupplyCurrentLimit(kSupplyCurrentLimit));
+          .withSupplyCurrentLimit(kSupplyCurrentLimit)
+          .withStatorCurrentLimit(kStatorCurrentLimit)
+          .withStatorCurrentLimitEnable(true));
 }
 

ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java

@@ -9,6 +9,7 @@ class AlgaeIOInputs {
     public double torque = 0.0;
     public double voltage = 0.0;
     public double temperature = 0.0;
+    public double stator = 0.0;
   }
 
   void setVoltage(double voltage);

ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOHardware.java

@@ -19,6 +19,7 @@ public class AlgaeIOHardware implements AlgaeIO {
   private final StatusSignal<Current> m_torqueSignal;
   private final StatusSignal<Temperature> m_tempSignal;
   private final StatusSignal<AngularVelocity> m_velocitySignal;
+  private final StatusSignal<Current> m_statorSignal;
 
   public AlgaeIOHardware() {
     m_motor = new TalonFX(AlgaeConstants.kMotorID);
@@ -30,13 +31,15 @@ public AlgaeIOHardware() {
     m_torqueSignal = m_motor.getTorqueCurrent();
     m_tempSignal = m_motor.getDeviceTemp();
     m_velocitySignal = m_motor.getVelocity();
+    m_statorSignal = m_motor.getStatorCurrent();
 
     StatusSignalUtil.registerRioSignals(
         m_voltageSignal,
         m_currentSignal,
         m_torqueSignal,
         m_tempSignal,
-        m_velocitySignal);
+        m_velocitySignal,
+        m_statorSignal);
   }
 
   public void updateInputs(AlgaeIOInputs inputs) {
@@ -50,6 +53,7 @@ public void updateInputs(AlgaeIOInputs inputs) {
     inputs.current = m_currentSignal.getValueAsDouble();
     inputs.torque = m_torqueSignal.getValueAsDouble();
     inputs.temperature = m_tempSignal.getValueAsDouble();
+    inputs.stator = m_statorSignal.getValueAsDouble();
   }
 
   public void setVoltage(double voltage) {

ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOInputsLogger.java

@@ -12,6 +12,8 @@ public AlgaeIOInputsLogger(AlgaeIOInputs inputs) {
     log.registerDouble("Current", () -> inputs.current);
     log.registerDouble("Torque", () -> inputs.torque);
     log.registerDouble("Voltage", () -> inputs.voltage);
+    log.registerDouble("Temperature", () -> inputs.temperature);
+    log.registerDouble("Stator Current", () -> inputs.stator);
   }
 
   public void log() {

ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java

@@ -25,7 +25,7 @@ public final class ClimberConstants {
 
   protected static final double kStowPosition = -0.25;
   protected static final double kClimbPosition = -0.110;
-  protected static final double kClimbReadyTolerance = -0.001;
+  protected static final double kClimbReadyTolerance = -0.004;
 
   protected static final double kFunnelOpenTime = 1.5;
 

ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralConstants.java

@@ -29,7 +29,7 @@ public class CoralConstants {
   protected static final double kL1LeftEjectVoltage = 2;
   protected static final double kL1RightEjectVoltage = 4;
 
-  protected static final double kSupplyCurrentLimit = 20.0;
+  protected static final double kSupplyCurrentLimit = 25.0;
 
   protected static final InvertedValue kInvertRight = InvertedValue.Clockwise_Positive;
 

ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java

@@ -25,6 +25,7 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.util.DriveFeedforwards;
+import com.pathplanner.lib.util.FlippingUtil;
 import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.swerve.SwerveSetpoint;
 import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator;
@@ -219,6 +220,10 @@ private void setPose(Pose2d pose) {
     resetPose(pose);
   }
 
+  public Command setLocalHeading(Rotation2d heading) {
+    return Commands.runOnce(() -> resetRotation(FieldUtils.getLocalRotation(heading))).ignoringDisable(true);
+  }
+
   private ChassisSpeeds getRobotRelativeSpeeds() {
     return getState().Speeds;
   }

ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java

@@ -110,8 +110,8 @@ public void periodic() {
    * Whether or not the elevator is above the "safe" range
    */
   public Trigger unsafe() {
-    return new Trigger(() -> m_inputs.position > ElevatorConstants.kUnsafeRange
-        || m_reference.position() > ElevatorConstants.kUnsafeRange);
+    return new Trigger(() -> m_inputs.position >= ElevatorConstants.kUnsafeRange
+        || m_reference.position() >= ElevatorConstants.kUnsafeRange);
   }
 
   protected void passive() {

ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java

@@ -50,7 +50,7 @@ public final class ElevatorConstants {
 
   protected static final double kZeroVoltage = -4.0;
 
-  protected static final double kUnsafeRange = ElevatorState.L2.position() + 2 * kInch;
+  protected static final double kUnsafeRange = ElevatorState.UpperReef.position();
 
   protected static final double kTolerance = 0.06;
 

ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java

@@ -18,7 +18,7 @@ public enum ElevatorState {
   /** Secondary L1 height for when a coral is already present */
   SecondaryL1(ElevatorState.L1.position() + 8 * ElevatorConstants.kInch),
   /** L2 height */
-  L2(4.016 + 3 * ElevatorConstants.kInch),
+  L2(4.016 + 4 * ElevatorConstants.kInch),
   /** L3 height */
   L3(7.257 - 4 * ElevatorConstants.kInch),
   /** L4 height */
@@ -28,7 +28,7 @@ public enum ElevatorState {
   /** Height to intake algae from lower reef */
   LowerReef(2.0),
   /** Height to intake algae from upper reef */
-  UpperReef(4.5);
+  UpperReef(4.5 - 1.5 * ElevatorConstants.kInch);
 
   private final double m_position;
 

ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotState.java

@@ -12,7 +12,7 @@ public enum PivotState {
   /** The angle to score at the net */
   Net(0.342),
   /** The "home" angle for the pivot */
-  Stow(0.343),
+  Stow(0.326),
   /** The angle for lollipop intake */
   HighGround(0.15);
 

ThriftyTest/src/main/java/frc/robot/superstructure/states/SeedAngle.java

@@ -0,0 +1,23 @@
+package frc.robot.superstructure.states;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.FieldConstants;
+import frc.robot.superstructure.EnterableState;
+import frc.robot.superstructure.Superstructure.Subsystems;
+
+public class SeedAngle implements EnterableState {
+    private final Rotation2d m_angle;
+
+    public SeedAngle(Rotation2d angle) {
+        m_angle = angle;
+    }
+
+    public static SeedAngle reverse() {
+        return new SeedAngle(FieldConstants.kStartHeading);
+    }
+
+    public Command build(Subsystems subsystems) {
+        return subsystems.drivetrain().setLocalHeading(m_angle);
+    }
+}

ThriftyTest/src/main/java/frc/robot/utils/FieldUtils.java

@@ -1,11 +1,12 @@
 package frc.robot.utils;
 
 import com.pathplanner.lib.util.FlippingUtil;
+import com.therekrab.autopilot.APTarget;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import com.therekrab.autopilot.APTarget;
 
 public class FieldUtils {
   /**
@@ -23,6 +24,14 @@ public static Pose2d getLocalPose(Pose2d localPose) {
     }
   }
 
+  public static Rotation2d getLocalRotation(Rotation2d rotation) {
+    if (DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Red)) {
+      return FlippingUtil.flipFieldRotation(rotation);
+    } else {
+      return rotation;
+    }
+  }
+
   /**
    * Flips the target if the driver station reports being on the red alliance
    */