Fixed drive and added more led stuff

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

@@ -111,16 +111,16 @@ public static final class ShooterConstants {
         new LoggedTunableNumber("Shooter Pivot Low Angle", 20);
 
     public static final LoggedTunableNumber kLeftFlywheelMidVelocity =
-        new LoggedTunableNumber("Shooter Left Flywheel Mid Velocity", 13.0);
+        new LoggedTunableNumber("Shooter Left Flywheel Mid Velocity", 9.0);
     public static final LoggedTunableNumber kRightFlywheelMidVelocity =
-        new LoggedTunableNumber("Shooter Right Flywheel Mid Velocity", 15.0);
+        new LoggedTunableNumber("Shooter Right Flywheel Mid Velocity", 11.0);
     public static final LoggedTunableNumber kPivotMidAngle =
         new LoggedTunableNumber("Shooter Pivot Mid Angle", 35);
 
     public static final LoggedTunableNumber kLeftFlywheelHighVelocity =
-        new LoggedTunableNumber("Shooter Left Flywheel High Velocity", 19.0);
+        new LoggedTunableNumber("Shooter Left Flywheel High Velocity", 9.0);
     public static final LoggedTunableNumber kRightFlywheelHighVelocity =
-        new LoggedTunableNumber("Shooter Right Flywheel High Velocity", 21.0);
+        new LoggedTunableNumber("Shooter Right Flywheel High Velocity", 11.0);
     public static final LoggedTunableNumber kPivotHighAngle =
         new LoggedTunableNumber("Shooter Pivot High Angle", 55);
   }
@@ -143,15 +143,9 @@ public static final class IntakeConstants {
     public static final LoggedTunableNumber kPivotI =
         new LoggedTunableNumber("Intake Pivot I", 0.0);
     public static final LoggedTunableNumber kPivotD =
-        new LoggedTunableNumber("Intake Pivot D", 0.0);
-    public static final double kPivotVelocity = 25.0;
-    public static final double kPivotAcceleration = 30.0;
-    public static final ProfiledPIDController kPivotController =
-        new ProfiledPIDController(
-            kPivotP.get(),
-            kPivotI.get(),
-            kPivotD.get(),
-            new Constraints(kPivotVelocity, kPivotAcceleration));
+        new LoggedTunableNumber("Intake Pivot D", 0.04);
+    public static final PIDController kPivotController =
+        new PIDController(kPivotP.get(), kPivotI.get(), kPivotD.get());
   }
 
   public static class IndexerConstants {

src/main/java/frc/robot/RobotContainer.java

@@ -133,10 +133,9 @@ private void configureSubsystems() {
       m_led = new Led(Ports.kLed, 20);
     }
 
-    m_shooter.setPivotAngle(ShooterConstants.kHomeAngle);
-    m_intake.setPivotAngle(IntakeConstants.kHomeAngle);
-
     m_robotState = RobotState.startInstance(m_drive, m_shooter, m_indexer, m_intake, m_led);
+
+    m_robotState.resetSetpoints();
   }
 
   private void configureControllers() {
@@ -146,6 +145,7 @@ private void configureControllers() {
     } else {
       m_driverControls = new DriverControlsXbox(1);
     }
+    m_driverControls = new DriverControlsXbox(1); // TODO: remove after testing
   }
 
   private void configureButtonBindings() {

src/main/java/frc/robot/RobotState.java

@@ -156,6 +156,16 @@ public void updateRobotState() {
     if (edu.wpi.first.wpilibj.RobotState.isDisabled()) {
       updatePrematchCheck();
     }
+
+    if (m_curAction == RobotAction.REV_SHOOTER_LOW
+        || m_curAction == RobotAction.REV_SHOOTER_MID
+        || m_curAction == RobotAction.REV_SHOOTER_HIGH) {
+      if (m_shooter.withinTolerance()) {
+        m_led.setState(LedState.SHOOTER_READY);
+      } else {
+        m_led.setState(LedState.SHOOTER_NOT_READY);
+      }
+    }
   }
 
   public void setCurAction(RobotAction action) {
@@ -168,11 +178,15 @@ public void setCurAction(RobotAction action) {
         m_shooter.setShooter(new ShooterPosition(ShooterConstants.kHomeAngle, 0.0, 0.0));
         m_intake.setPivotAngle(IntakeConstants.kHomeAngle);
         m_intake.setRollerVoltage(0.0);
+        m_indexer.setKickerVoltage(0.0);
+        m_indexer.setFeederVoltage(0.0);
         break;
       case INTAKE:
+        m_shooter.setShooter(new ShooterPosition(ShooterConstants.kHomeAngle, 0.0, 0.0));
         m_intake.setPivotAngle(IntakeConstants.kDeployedAngle);
         m_intake.setRollerVoltage(IntakeConstants.kDeployRollerVoltage);
         m_indexer.setFeederVoltage(IndexerConstants.kFeederVoltage);
+        m_indexer.setKickerVoltage(0.0);
         break;
       case REV_SHOOTER_LOW:
         m_shooter.setShooter(
@@ -181,6 +195,8 @@ public void setCurAction(RobotAction action) {
                 ShooterConstants.kLeftFlywheelLowVelocity.get(),
                 ShooterConstants.kRightFlywheelLowVelocity.get()));
         m_intake.setPivotAngle(IntakeConstants.kDeployedAngle);
+        m_indexer.setFeederVoltage(0.0);
+        m_indexer.setKickerVoltage(0.0);
         break;
       case REV_SHOOTER_MID:
         m_shooter.setShooter(
@@ -189,6 +205,8 @@ public void setCurAction(RobotAction action) {
                 ShooterConstants.kLeftFlywheelMidVelocity.get(),
                 ShooterConstants.kRightFlywheelMidVelocity.get()));
         m_intake.setPivotAngle(IntakeConstants.kDeployedAngle);
+        m_indexer.setKickerVoltage(0.0);
+        m_indexer.setFeederVoltage(0.0);
         break;
       case REV_SHOOTER_HIGH:
         m_shooter.setShooter(
@@ -197,6 +215,8 @@ public void setCurAction(RobotAction action) {
                 ShooterConstants.kLeftFlywheelHighVelocity.get(),
                 ShooterConstants.kRightFlywheelHighVelocity.get()));
         m_intake.setPivotAngle(IntakeConstants.kDeployedAngle);
+        m_indexer.setKickerVoltage(0.0);
+        m_indexer.setFeederVoltage(0.0);
         break;
     }
   }
@@ -252,6 +272,7 @@ public void onEnableAutonomous() {
 
   public void onDisable() {
     m_led.setState(LedState.DISABLED_NOT_READY);
+    resetSetpoints();
   }
 
   public void setPrematchCheckValue(String key, boolean value) {
@@ -269,4 +290,12 @@ public void addVisionObservation(VisionObservation observation) {
   public Pose2d getOdometryPose() {
     return m_drive.getPose();
   }
+
+  public void resetSetpoints() {
+    m_shooter.setShooter(new ShooterPosition(ShooterConstants.kHomeAngle, 0.0, 0.0));
+    m_intake.setPivotAngle(IntakeConstants.kHomeAngle);
+    m_intake.setRollerVoltage(0.0);
+    m_indexer.setFeederVoltage(0.0);
+    m_indexer.setKickerVoltage(0.0);
+  }
 }

src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java

@@ -25,7 +25,7 @@
 
 /** IO implementation for Pigeon2 */
 public class GyroIOPigeon2 implements GyroIO {
-  private final Pigeon2 pigeon = new Pigeon2(20);
+  private final Pigeon2 pigeon = new Pigeon2(22, "Drivetrain");
   private final StatusSignal<Double> yaw = pigeon.getYaw();
   private final Queue<Double> yawPositionQueue;
   private final Queue<Double> yawTimestampQueue;

src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java

@@ -15,7 +15,6 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.VoltageOut;
@@ -72,25 +71,25 @@ public ModuleIOTalonFX(int index) {
         driveTalon = new TalonFX(10, "Drivetrain");
         turnTalon = new TalonFX(11, "Drivetrain");
         cancoder = new CANcoder(12, "Drivetrain");
-        absoluteEncoderOffset = new Rotation2d(2.827); // MUST BE CALIBRATED
+        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
         break;
       case 1:
         driveTalon = new TalonFX(7, "Drivetrain");
         turnTalon = new TalonFX(8, "Drivetrain");
         cancoder = new CANcoder(9, "Drivetrain");
-        absoluteEncoderOffset = new Rotation2d(-2.945); // MUST BE CALIBRATED
+        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
         break;
       case 2:
         driveTalon = new TalonFX(4, "Drivetrain");
         turnTalon = new TalonFX(5, "Drivetrain");
         cancoder = new CANcoder(6, "Drivetrain");
-        absoluteEncoderOffset = new Rotation2d(0.175); // MUST BE CALIBRATED
+        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
         break;
       case 3:
         driveTalon = new TalonFX(1, "Drivetrain");
         turnTalon = new TalonFX(2, "Drivetrain");
         cancoder = new CANcoder(3, "Drivetrain");
-        absoluteEncoderOffset = new Rotation2d(1.226); // MUST BE CALIBRATED
+        absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
         break;
       default:
         throw new RuntimeException("Invalid module index");
@@ -108,7 +107,7 @@ public ModuleIOTalonFX(int index) {
     turnTalon.getConfigurator().apply(turnConfig);
     setTurnBrakeMode(true);
 
-    cancoder.getConfigurator().apply(new CANcoderConfiguration());
+    // cancoder.getConfigurator().apply(new CANcoderConfiguration());
 
     timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
 

src/main/java/frc/robot/subsystems/intake/Intake.java

@@ -1,6 +1,6 @@
 package frc.robot.subsystems.intake;
 
-import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -22,9 +22,9 @@ public class Intake extends SubsystemBase {
   public final RollerInputsAutoLogged m_rollerInputs;
   public final IntakePivotInputsAutoLogged m_pivotInputs;
 
-  private ProfiledPIDController m_pivotController;
+  private PIDController m_pivotController;
 
-  public Intake(RollerIO io, IntakePivotIO pivotIO, ProfiledPIDController pivotController) {
+  public Intake(RollerIO io, IntakePivotIO pivotIO, PIDController pivotController) {
     m_rollerIO = io;
     m_pivotIO = pivotIO;
     m_pivotController = pivotController;
@@ -57,13 +57,12 @@ public void periodic() {
 
     Logger.recordOutput("Intake/Pivot/PIDVoltage", pivotPidVoltage);
     Logger.recordOutput(
-        "Intake/Pivot/DesiredAngle",
-        Units.radiansToDegrees(m_pivotController.getSetpoint().position));
+        "Intake/Pivot/DesiredAngle", Units.radiansToDegrees(m_pivotController.getSetpoint()));
     Logger.recordOutput(
         "Intake/Pivot/CurrentAngle", Units.radiansToDegrees(m_pivotInputs.curAngle));
 
     // ready for match
-    RobotState.getInstance().setPrematchCheckValue("IntakePivot", m_pivotController.atGoal());
+    RobotState.getInstance().setPrematchCheckValue("IntakePivot", m_pivotController.atSetpoint());
     RobotState.getInstance()
         .setPrematchCheckValue("RollersStill", Math.abs(m_rollerInputs.curVelocity) < 0.1);
   }
@@ -73,15 +72,15 @@ public void setRollerVoltage(double voltage) {
   }
 
   public void setPivotAngle(Rotation2d angle) {
-    m_pivotController.setGoal(angle.getRadians());
+    m_pivotController.setSetpoint(angle.getRadians());
   }
 
   public Command runRollerCommand(double voltage) {
     return Commands.runOnce(() -> m_rollerIO.setVoltage(voltage));
   }
 
   public Command setPivotAngleCommand(Rotation2d angle) {
-    return Commands.runOnce(() -> m_pivotController.setGoal(angle.getRadians()));
+    return Commands.runOnce(() -> m_pivotController.setSetpoint(angle.getRadians()));
   }
 
   public Rotation2d getPivotAngle() {

src/main/java/frc/robot/subsystems/led/Led.java

@@ -15,6 +15,8 @@ public static enum LedState {
     OFF,
     AUTONOMOUS,
     TELEOP,
+    SHOOTER_NOT_READY,
+    SHOOTER_READY,
     DISABLED_NOT_READY,
     DISABLED_READY
   }
@@ -47,6 +49,9 @@ public void setState(LedState state) {
       case OFF:
         setSolidColor(Color.kBlack);
         break;
+      case SHOOTER_READY:
+        setSolidColor(Color.kYellow);
+        break;
       case AUTONOMOUS:
         setSolidColor(Color.kBlue);
         break;
@@ -56,6 +61,7 @@ public void setState(LedState state) {
       case DISABLED_NOT_READY:
         setSolidColor(Color.kRed);
         break;
+      case SHOOTER_NOT_READY:
       case DISABLED_READY:
         setSolidColor(Color.kGreen);
         break;

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -207,4 +207,10 @@ public Command setShooterCommand(ShooterPosition position) {
   public Rotation2d getPivotAngle() {
     return Rotation2d.fromRadians(m_pivotInputs.curAngle);
   }
+
+  public boolean withinTolerance() {
+    return m_pivotController.atGoal()
+        && m_leftFlywheelController.atSetpoint()
+        && m_rightFlywheelController.atSetpoint();
+  }
 }