Intake pivot

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

@@ -198,6 +198,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 {
@@ -300,9 +320,9 @@ public static final class CANConstants {
     public static final int HOOD_MOTOR_ID = 15;
     public static final int HOOD_ENCODER_ID = 16;
 
-    public static final int INTAKE_WHEEL_MOTOR_ID = 17;
-    public static final int INTAKE_PIVOT_MOTOR_ID = 18;
-    public static final int INTAKE_PIVOT_ENCODER_ID = 19;
+    public static final int INTAKE_WHEEL_MOTOR_ID = 18;
+    public static final int INTAKE_PIVOT_MOTOR_ID = 19;
+    public static final int INTAKE_PIVOT_ENCODER_ID = 20;
 
     public static final int INDEXER_WHEEL_ID = 20;
     public static final int INDEXER_BELT_ID = 21;

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

@@ -42,6 +42,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.shooter.Flywheel;
 import org.team2342.frc.subsystems.shooter.Hood;
@@ -64,6 +65,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 Wheels wheels;
   @Getter private final Flywheel flywheel;
@@ -113,16 +115,19 @@ public RobotContainer() {
                 new SmartMotorIOTalonFX(
                     CANConstants.TURRET_ID,
                     TurretConstants.TURRET_CONFIG.withPIDFFConfigs(TurretConstants.PID_CONFIG)));
-        flywheel =
-            new Flywheel(
-                new SmartMotorIOTalonFX(
-                    CANConstants.FLYWHEEL_MOTOR_ID,
-                    ShooterConstants.FLYWHEEL_CONFIG.withPIDFFConfigs(
-                        ShooterConstants.FLYWHEEL_PID_CONFIGS)));
-
+        flywheel = new Flywheel(new SmartMotorIO() {});
+        hood = new Hood(new SmartMotorIO() {});
         indexer = new Indexer(new DumbMotorIO() {}, new DumbMotorIO() {});
         wheels = new Wheels(new DumbMotorIO() {});
-        hood = new Hood(new SmartMotorIO() {});
+        pivot =
+            new Pivot(
+                new SmartMotorIOTalonFX(
+                    CANConstants.INTAKE_PIVOT_MOTOR_ID,
+                    IntakeConstants.PIVOT_MOTOR_CONFIG.withPIDFFConfigs(
+                        IntakeConstants.PIVOT_MOTOR_PID_CONFIGS)));
+                new SmartMotorIOTalonFX(
+                    CANConstants.TURRET_ID,
+                    TurretConstants.TURRET_CONFIG.withPIDFFConfigs(TurretConstants.PID_CONFIG));
 
         LoggedPowerDistribution.getInstance(CANConstants.PDH_ID, ModuleType.kRev);
         break;
@@ -177,6 +182,7 @@ public RobotContainer() {
                     ShooterConstants.HOOD_SIM,
                     1));
         turret = new Turret(new SmartMotorIO() {});
+        pivot = new Pivot(new SmartMotorIO() {});
 
         break;
 
@@ -196,6 +202,7 @@ public RobotContainer() {
                 new VisionIO() {});
         indexer = new Indexer(new DumbMotorIO() {}, new DumbMotorIO() {});
         wheels = new Wheels(new DumbMotorIO() {});
+        pivot = new Pivot(new SmartMotorIO() {});
         flywheel = new Flywheel(new SmartMotorIO() {});
         hood = new Hood(new SmartMotorIO() {});
         turret = new Turret(new SmartMotorIO() {});

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");
+  }
+}