Merging main into shooter_tuning

src/main/java/org/team2342/frc/Constants.java

@@ -170,6 +170,26 @@ public static final class IntakeConstants {
         new DCMotorSim(
             LinearSystemId.createDCMotorSystem(INTAKE_WHEELS_SIM_MOTOR, 0.003, 1),
             INTAKE_WHEELS_SIM_MOTOR);
+
+    public static final double PIVOT_GEAR_RATIO = 40;
+    public static final double MIN_ANGLE = 0.1;
+    public static final double MAX_ANGLE = 2.23;
+
+    public static final PIDFFConfigs PIVOT_MOTOR_PID_CONFIGS =
+        new PIDFFConfigs().withKP(45).withKI(15).withKD(7);
+    public static final SmartMotorConfig PIVOT_MOTOR_CONFIG =
+        new SmartMotorConfig()
+            .withGearRatio(1.0)
+            .withControlType(ControlType.PROFILED_POSITION)
+            .withIdleMode(IdleMode.BRAKE)
+            .withMotorInverted(false)
+            .withSupplyCurrentLimit(40.0)
+            .withFeedbackConfig(
+                FeedbackConfig.fused(
+                    CANConstants.INTAKE_PIVOT_ENCODER_ID, PIVOT_GEAR_RATIO, 0.173, true))
+            .withProfileConstraintsRad(
+                new TrapezoidProfile.Constraints(
+                    Units.degreesToRadians(1800), Units.degreesToRadians(540)));
   }
 
   public static final class ShooterConstants {

src/main/java/org/team2342/frc/RobotContainer.java

@@ -44,6 +44,7 @@
 import org.team2342.frc.subsystems.drive.ModuleIOSim;
 import org.team2342.frc.subsystems.drive.ModuleIOTalonFX;
 import org.team2342.frc.subsystems.indexer.Indexer;
+import org.team2342.frc.subsystems.intake.Pivot;
 import org.team2342.frc.subsystems.intake.Wheels;
 import org.team2342.frc.subsystems.leds.LEDSubsystem;
 import org.team2342.frc.subsystems.shooter.Flywheel;
@@ -71,6 +72,7 @@
 public class RobotContainer {
   @Getter private final Drive drive;
   @Getter private final Vision vision;
+  @Getter private final Pivot pivot;
   @Getter private final Indexer indexer;
   @Getter private final Kicker kicker;
   @Getter private final Wheels wheels;
@@ -137,6 +139,12 @@ public RobotContainer() {
             new Indexer(
                 new DumbMotorIOTalonFXFOC(
                     CANConstants.INDEXER_MOTOR_ID, IndexerConstants.INDEXER_MOTOR_CONFIG));
+        pivot =
+            new Pivot(
+                new SmartMotorIOTalonFX(
+                    CANConstants.INTAKE_PIVOT_MOTOR_ID,
+                    IntakeConstants.PIVOT_MOTOR_CONFIG.withPIDFFConfigs(
+                        IntakeConstants.PIVOT_MOTOR_PID_CONFIGS)));
         wheels =
             new Wheels(
                 new DumbMotorIOTalonFX(
@@ -190,6 +198,7 @@ public RobotContainer() {
                     1));
         kicker = new Kicker(new DumbMotorIO() {});
         turret = new Turret(new SmartMotorIO() {});
+        pivot = new Pivot(new SmartMotorIO() {});
 
         conductor = new Conductor(flywheel, turret, drive::getPose, drive::getChassisSpeeds);
         leds =
@@ -213,6 +222,7 @@ public RobotContainer() {
                 new VisionIO() {});
         indexer = new Indexer(new DumbMotorIO() {});
         wheels = new Wheels(new DumbMotorIO() {});
+        pivot = new Pivot(new SmartMotorIO() {});
         flywheel = new Flywheel(new SmartMotorIO() {});
         turret = new Turret(new SmartMotorIO() {});
         kicker = new Kicker(new DumbMotorIO() {});

src/main/java/org/team2342/frc/subsystems/intake/Pivot.java

@@ -0,0 +1,67 @@
+// Copyright (c) 2026 Team 2342
+// https://github.com/FRCTeamPhoenix
+//
+// This source code is licensed under the MIT License.
+// See the LICENSE file in the root directory of this project.
+
+package org.team2342.frc.subsystems.intake;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+import org.team2342.frc.Constants.IntakeConstants;
+import org.team2342.lib.logging.ExecutionLogger;
+import org.team2342.lib.motors.smart.SmartMotorIO;
+import org.team2342.lib.motors.smart.SmartMotorIOInputsAutoLogged;
+
+public class Pivot extends SubsystemBase {
+
+  private final SmartMotorIO pivotMotor;
+  private final SmartMotorIOInputsAutoLogged pivotMotorInputs = new SmartMotorIOInputsAutoLogged();
+
+  @AutoLogOutput(key = "Intake/Pivot/TargetAngle")
+  private double goal = 2.23;
+
+  private final Alert pivotMotorAlert = new Alert("Pivot motor is disconnected!", AlertType.kError);
+
+  public Pivot(SmartMotorIO pivotMotor) {
+    this.pivotMotor = pivotMotor;
+    setName("Intake/Pivot");
+    setDefaultCommand(run(() -> pivotMotor.runVoltage(0.0)));
+  }
+
+  @Override
+  public void periodic() {
+    pivotMotor.updateInputs(pivotMotorInputs);
+    Logger.processInputs("Intake/Pivot", pivotMotorInputs);
+    pivotMotorAlert.set(!pivotMotorInputs.motorsConnected[0]);
+    ExecutionLogger.log("Intake/Pivot");
+  }
+
+  public void runAngle(double angle) {
+    goal = MathUtil.clamp(angle, IntakeConstants.MIN_ANGLE, IntakeConstants.MAX_ANGLE);
+    pivotMotor.runPosition(goal);
+  }
+
+  public Command goToAngle(double angle) {
+    return run(() -> runAngle(angle))
+        .until(() -> Math.abs(goal - angle) <= 0.01)
+        .withName("Pivot Go To Angle");
+  }
+
+  public Command runVoltage(double voltage) {
+    return run(() -> pivotMotor.runVoltage(voltage)).withName("Pivot Run Voltage");
+  }
+
+  public Command holdAngle(double angle) {
+    return run(() -> runAngle(angle)).withName("Pivot Hold Angle");
+  }
+
+  public Command stop() {
+    return runOnce(() -> pivotMotor.runVoltage(0.0)).withName("Pivot Stop");
+  }
+}