Solo c2

src/main/java/frc/robot/generated/CompTunerConstants.java

@@ -55,7 +55,7 @@ public class CompTunerConstants {
 
   // The stator current at which the wheels start to slip;
   // This needs to be tuned to your individual robot
-  private static final Current kSlipCurrent = Amps.of(120.0);
+  private static final Current kSlipCurrent = Amps.of(80.0);
 
   // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
   // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.

src/main/java/frc/robot/sensors/ElevatorLight.java

@@ -16,7 +16,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Hardware;
-import frc.robot.util.ScoringMode;
+import frc.robot.util.SoloScoringMode;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
@@ -82,13 +82,15 @@ public Command animate(LEDPattern animation, String name) {
         .withName("Animate" + name);
   }
 
-  public Command showScoringMode(Supplier<ScoringMode> scoringMode) {
+  public Command showScoringMode(Supplier<SoloScoringMode> scoringMode) {
     return run(() -> {
-          ScoringMode currentMode = scoringMode.get();
-          if (currentMode == ScoringMode.ALGAE) {
+          SoloScoringMode currentMode = scoringMode.get();
+          if (currentMode == SoloScoringMode.ALGAE_IN_CLAW) {
             updateLEDs(LEDPattern.solid(Color.kTeal));
-          } else {
+          } else if (currentMode == SoloScoringMode.CORAL_IN_CLAW) {
             updateLEDs(LEDPattern.solid(Color.kWhite));
+          } else if (currentMode == SoloScoringMode.NO_GAME_PIECE) {
+            updateLEDs(LEDPattern.solid(Color.kDeepPink));
           }
         })
         .withName("Animate Scoring Mode");

src/main/java/frc/robot/sensors/IntakeSensor.java

@@ -15,8 +15,8 @@ public class IntakeSensor {
 
   private final LaserCan intakeSensor;
   // VALUES ARE IN METERS
-  private static final double INTAKE_LOWER_LIMIT = 0.010;
-  private static final double INTAKE_UPPER_LIMIT = 0.1;
+  private static final double INTAKE_LOWER_LIMIT = 0.005;
+  private static final double INTAKE_UPPER_LIMIT = 0.05;
 
   public IntakeSensor() {
     intakeSensor = new LaserCan(Hardware.GROUND_INTAKE_SENSOR);
@@ -29,7 +29,7 @@ public IntakeSensor() {
   private void ConfigureSensor(LaserCan Sensor) {
     try {
       Sensor.setRangingMode(LaserCan.RangingMode.SHORT);
-      Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16));
+      Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(4, 4, 16, 16));
       Sensor.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS);
     } catch (ConfigurationFailedException e) {
       System.out.println("Configuration Failed! " + e);

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

@@ -195,9 +195,9 @@ public void factoryDefaults() {
     talonFXConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake;
 
     // enabling current limits
-    talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 80; // starting low for testing
+    talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 80;
     talonFXConfiguration.CurrentLimits.StatorCurrentLimitEnable = true;
-    talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 40; // starting low for testing
+    talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 20;
     talonFXConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true;
 
     // PID

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

@@ -56,7 +56,7 @@ public enum TargetPositions {
   private final double CLIMB_HOLD_CLIMBED = -0.0705;
   private final double CLIMB_IN_SPEED = -0.75;
 
-  private final double climbInKp = 30;
+  private final double climbInKp = 50;
   private final double climbOutKp = 50;
 
   // positions for relation between motor encoder and WCP encoder

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

@@ -16,7 +16,7 @@ public class GroundSpinny extends SubsystemBase {
   public static final double GROUND_INTAKE_JITTER_SPEED = 1;
   public static final double FUNNEL_INTAKE_SPEED = -2;
   public static final double QUICK_HANDOFF_EXTAKE_SPEED = 1;
-  private static final double STATOR_CURRENT_STALL_THRESHOLD = 20;
+  private static final double STATOR_CURRENT_STALL_THRESHOLD = 50;
 
   // TalonFX
   private final TalonFX motor;

src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java

@@ -0,0 +1,104 @@
+package frc.robot.subsystems.auto;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Controls;
+import frc.robot.subsystems.SuperStructure;
+import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain;
+import frc.robot.util.ScoringMode;
+import java.util.List;
+
+/**
+ * Command that automatically moves the elevator to the proper height for the nearest algae based on
+ * alliance and current robot pose.
+ */
+public class AutoAlgaeHeights extends Command {
+
+  private final CommandSwerveDrivetrain drivebase;
+  private final SuperStructure s;
+  private final Controls c;
+
+  private static final AprilTagFieldLayout aprilTagFieldLayout =
+      AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
+
+  private static final List<Pose2d> blueAlgaePoses =
+      List.of(
+          aprilTagFieldLayout.getTagPose(17).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(19).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(21).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(18).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(20).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(22).get().toPose2d());
+
+  private static final List<Pose2d> redAlgaePoses =
+      List.of(
+          aprilTagFieldLayout.getTagPose(6).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(8).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(10).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(7).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(9).get().toPose2d(),
+          aprilTagFieldLayout.getTagPose(11).get().toPose2d());
+
+  public AutoAlgaeHeights(CommandSwerveDrivetrain drivebase, SuperStructure s, Controls c) {
+    this.drivebase = drivebase;
+    this.s = s;
+    this.c = c;
+  }
+
+  public static Command autoAlgaeIntakeCommand(
+      CommandSwerveDrivetrain drivebase, SuperStructure s, Controls c) {
+    return new AutoAlgaeHeights(drivebase, s, c);
+  }
+
+  private static boolean isBlue() {
+    return DriverStation.getAlliance()
+        .map(alliance -> alliance.equals(Alliance.Blue))
+        .orElse(false);
+  }
+
+  private static Pose2d getNearestAlgae(Pose2d robotPose) {
+    List<Pose2d> poses = isBlue() ? blueAlgaePoses : redAlgaePoses;
+    return robotPose.nearest(poses);
+  }
+
+  private String aprilTagToAlgaeHeight(Pose2d algaePose) {
+    // Map poses to two heights only
+    List<Pose2d> poses = isBlue() ? blueAlgaePoses : redAlgaePoses;
+    int index = poses.indexOf(algaePose);
+
+    if (index >= 3) {
+      return "top"; // upper reef
+    } else {
+      return "low"; // lower reef
+    }
+  }
+
+  private String activeCommand = "";
+
+  @Override
+  public void execute() {
+    Pose2d robotPose = drivebase.getState().Pose;
+    Pose2d nearestAlgae = getNearestAlgae(robotPose);
+
+    String targetHeight = aprilTagToAlgaeHeight(nearestAlgae);
+
+    if (!(activeCommand.equals(targetHeight))) {
+      if (targetHeight.equals("top")) {
+        s.algaeLevelThreeFourIntake().schedule();
+        activeCommand = "top";
+      } else {
+        s.algaeLevelTwoThreeIntake().schedule();
+        activeCommand = "low";
+      }
+    }
+  }
+
+  @Override
+  public boolean isFinished() {
+    return c.intakeMode != ScoringMode.ALGAE;
+  }
+}

src/main/java/frc/robot/util/SoloScoringMode.java

@@ -0,0 +1,7 @@
+package frc.robot.util;
+
+public enum SoloScoringMode {
+  CORAL_IN_CLAW,
+  ALGAE_IN_CLAW,
+  NO_GAME_PIECE;
+}