Major changes

Author: Toomy422

src/main/java/frc/robot/BuildConstants.java

@@ -5,12 +5,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "spike-rewrite";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 12;
-  public static final String GIT_SHA = "daba3e29189bf5cfe92c48d83e22c333499d7057";
-  public static final String GIT_DATE = "2025-08-06 23:05:38 EDT";
+  public static final int GIT_REVISION = 14;
+  public static final String GIT_SHA = "f356523e7c158ffe83ee8625cebc234a3cc908b5";
+  public static final String GIT_DATE = "2025-08-07 23:17:27 EDT";
   public static final String GIT_BRANCH = "main";
-  public static final String BUILD_DATE = "2025-08-07 23:10:24 EDT";
-  public static final long BUILD_UNIX_TIME = 1754622624086L;
+  public static final String BUILD_DATE = "2025-08-11 21:53:21 EDT";
+  public static final long BUILD_UNIX_TIME = 1754963601492L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

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

@@ -131,6 +131,14 @@ public static final class DriveConstants {
     public static final LoggedTunableNumber kMeshDrivePriority =
         new LoggedTunableNumber("Meshed Drive Priority", 0.3);
 
+    public static final LoggedTunableNumber kDriveP = new LoggedTunableNumber("Drive P", 0.0);
+    public static final LoggedTunableNumber kDriveI = new LoggedTunableNumber("Drive I", 0.0);
+    public static final LoggedTunableNumber kDriveD = new LoggedTunableNumber("Drive D", 0.0);
+
+    public static final LoggedTunableNumber kTurnP = new LoggedTunableNumber("Drive Turn P", 3.0);
+    public static final LoggedTunableNumber kTurnI = new LoggedTunableNumber("Drive Turn I", 0.0);
+    public static final LoggedTunableNumber kTurnD = new LoggedTunableNumber("Drive Turn D", 0.0);
+
     public static final LoggedTunableNumber kAutoscoreDeployDistance =
         new LoggedTunableNumber("Autoscore Deploy Distance", 42.0);
     public static final LoggedTunableNumber kAutoscoreOuttakeDistance =
@@ -157,37 +165,38 @@ public static final class ShooterConstants {
     public static final LoggedTunableNumber kIdleVoltage =
         new LoggedTunableNumber("Shooter idleVoltage", 0.0);
     public static final LoggedTunableNumber kTopRPS =
-        new LoggedTunableNumber("Shooter topRPM", 20.0);
+        new LoggedTunableNumber("Shooter topRPS", 20.0);
     public static final LoggedTunableNumber kBottomRPS =
-        new LoggedTunableNumber("Shooter bottomRPM", 85.0);
+        new LoggedTunableNumber("Shooter bottomRPS", 85.0);
     public static final LoggedTunableNumber kRejectingVoltage =
-        new LoggedTunableNumber("Shooter rejectingVoltage", 7.0);
+        new LoggedTunableNumber("Shooter rejectingVoltage", 4.0);
     public static final LoggedTunableNumber kTopAmpVelocity =
         new LoggedTunableNumber("Shooter topAmpVelocity", -3.5);
     public static final LoggedTunableNumber kBottomAmpVelocity =
         new LoggedTunableNumber("Shooter bottomAmpVelocity", 58.0);
 
     public static final LoggedTunableNumber kTopShooterP =
-        new LoggedTunableNumber("Shooter TopP", 0.0);
+        new LoggedTunableNumber("Shooter TopP", 0.005);
     public static final LoggedTunableNumber kTopShooterI =
         new LoggedTunableNumber("Shooter TopI", 0.0);
     public static final LoggedTunableNumber kTopShooterD =
         new LoggedTunableNumber("Shooter TopD", 0.0);
     public static final LoggedTunableNumber kBottomShooterP =
-        new LoggedTunableNumber("Shooter BottomP", 0.0);
+        new LoggedTunableNumber("Shooter BottomP", 0.005);
     public static final LoggedTunableNumber kBottomShooterI =
         new LoggedTunableNumber("Shooter BottomI", 0.0);
     public static final LoggedTunableNumber kBottomShooterD =
         new LoggedTunableNumber("Shooter BottomD", 0.0);
 
-    public static final LoggedTunableNumber kTopKs = new LoggedTunableNumber("Shooter TopKs", 0.0);
-    public static final LoggedTunableNumber kTopKv = new LoggedTunableNumber("Shooter TopKv", 0.0);
+    public static final LoggedTunableNumber kTopKs = new LoggedTunableNumber("Shooter TopKs", 0.14);
+    public static final LoggedTunableNumber kTopKv =
+        new LoggedTunableNumber("Shooter TopKv", 0.125);
     public static final LoggedTunableNumber kBottomKs =
-        new LoggedTunableNumber("Shooter BottomKs", 0.0);
+        new LoggedTunableNumber("Shooter BottomKs", 0.14);
     public static final LoggedTunableNumber kBottomKv =
-        new LoggedTunableNumber("Shooter BottomKv", 0.0);
+        new LoggedTunableNumber("Shooter BottomKv", 0.125);
 
-    public static final double kVelocityTolerance = 0.1; // in RPM
+    public static final double kVelocityTolerance = 0.1;
 
     // sim
     public static final DCMotor kTopDCMotor = DCMotor.getNEO(1);
@@ -223,11 +232,11 @@ public static final class IndexerConstants {
     public static final LoggedTunableNumber kIdleVoltage =
         new LoggedTunableNumber("Indexer idleVoltage", 0.0);
     public static final LoggedTunableNumber kIntakingVoltage =
-        new LoggedTunableNumber("Indexer intakingVoltage", 12.0);
+        new LoggedTunableNumber("Indexer intakingVoltage", -6.0);
     public static final LoggedTunableNumber kShootingVoltage =
-        new LoggedTunableNumber("Indexer shootingVoltage", 8.0);
+        new LoggedTunableNumber("Indexer shootingVoltage", -8.0);
     public static final LoggedTunableNumber kVomitVoltage =
-        new LoggedTunableNumber("Indexer vomitVoltage", -8.0);
+        new LoggedTunableNumber("Indexer vomitVoltage", 6.0);
 
     // sim
     public static final DCMotor kDCMotor = DCMotor.getNEO(1);
@@ -239,9 +248,9 @@ public static final class IntakeConstants {
     public static final LoggedTunableNumber kIdleVoltage =
         new LoggedTunableNumber("Intake idleVoltage", 0.0);
     public static final LoggedTunableNumber kIntakingVoltage =
-        new LoggedTunableNumber("Intake intakingVoltage", 10.0);
+        new LoggedTunableNumber("Intake intakingVoltage", -6.0);
     public static final LoggedTunableNumber kVomitVoltage =
-        new LoggedTunableNumber("Intake vomitVoltage", -8.0);
+        new LoggedTunableNumber("Intake vomitVoltage", 7.0);
 
     // sim
     public static final DCMotor kDCMotor = DCMotor.getNEO(1);
@@ -250,32 +259,33 @@ public static final class IntakeConstants {
   }
 
   public static final class Ports {
-    public static final int kFrontLeftDrive = 0;
-    public static final int kFrontLeftTurn = 1;
-    public static final int kFrontLeftCancoder = 2;
+    public static final int kFrontLeftDrive = 1;
+    public static final int kFrontLeftTurn = 2;
+    public static final int kFrontLeftCancoder = 3;
 
-    public static final int kFrontRightDrive = 3;
-    public static final int kFrontRightTurn = 4;
-    public static final int kFrontRightCancoder = 5;
+    public static final int kFrontRightDrive = 4;
+    public static final int kFrontRightTurn = 5;
+    public static final int kFrontRightCancoder = 6;
 
-    public static final int kBackLeftDrive = 6;
-    public static final int kBackLeftTurn = 7;
-    public static final int kBackLeftCancoder = 8;
+    public static final int kBackLeftDrive = 7;
+    public static final int kBackLeftTurn = 8;
+    public static final int kBackLeftCancoder = 9;
 
-    public static final int kBackRightDrive = 9;
-    public static final int kBackRightTurn = 10;
-    public static final int kBackRightCancoder = 11;
+    public static final int kBackRightDrive = 10;
+    public static final int kBackRightTurn = 11;
+    public static final int kBackRightCancoder = 12;
 
-    public static final int kIndexer = 12;
-    public static final int kPhotoelectric1 = 13;
-    public static final int kPhotoelectric2 = 14;
+    public static final int kPigeon = 22;
 
-    public static final int kIntake = 15;
+    public static final String kCanivoreName = "Drivetrain";
 
-    public static final int kTopShooter = 16;
-    public static final int kBottomShooter = 17;
+    public static final int kIntakeNeo = 13;
+    public static final int kIndexerNeo = 14;
+    public static final int kTopFlywheel = 15;
+    public static final int kBottomFlywheel = 16;
 
-    public static final int kPigeon = 22;
+    public static final int kPhotoElectricOne = 8;
+    public static final int kPhotoElectricTwo = 9;
 
     public static final String kDriveCanivoreName = "Drivetrain";
 

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

@@ -62,11 +62,12 @@ private void configureSubsystems() {
                 new ModuleIOSparkMax(3));
         m_indexer =
             new Indexer(
-                new IndexerIONeo(Ports.kIndexer, Ports.kPhotoelectric1, Ports.kPhotoelectric2));
-        m_intake = new Intake(new IntakeIONeo(Ports.kIntake));
+                new IndexerIONeo(
+                    Ports.kIndexerNeo, Ports.kPhotoElectricOne, Ports.kPhotoElectricTwo));
+        m_intake = new Intake(new IntakeIONeo(Ports.kIntakeNeo));
         m_shooter =
             new Shooter(
-                new ShooterIONeo(Ports.kTopShooter, Ports.kBottomShooter),
+                new ShooterIONeo(Ports.kTopFlywheel, Ports.kBottomFlywheel),
                 new PIDController(
                     ShooterConstants.kTopShooterP.getAsDouble(),
                     ShooterConstants.kTopShooterI.getAsDouble(),

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

@@ -55,7 +55,7 @@ private RobotState(Drive drive, Shooter shooter, Indexer indexer, Intake intake)
     Map<RobotAction, Runnable> periodicHash = new HashMap<>();
 
     periodicHash.put(RobotAction.kTeleopDefault, () -> {});
-    periodicHash.put(RobotAction.kIntake, () -> {});
+    periodicHash.put(RobotAction.kIntake, this::intakingPeriodic);
     periodicHash.put(RobotAction.kVomitting, () -> {});
     periodicHash.put(RobotAction.kAlignShooting, this::alignShootingPeriodic);
     periodicHash.put(RobotAction.kSubwooferShooting, this::revPeriodic);
@@ -123,6 +123,12 @@ public void ampPeriodic() {
     m_drive.setDesiredHeading(Rotation2d.fromDegrees(90));
   }
 
+  public void intakingPeriodic() {
+    if (m_indexer.indexerHasGamePiece()) {
+      m_intake.updateState(IntakeState.kIdle);
+    }
+  }
+
   public void updateAction(RobotAction action) {
     DriveProfiles newDriveProfile = DriveProfiles.kDefault;
     IndexerState newIndexerState = IndexerState.kIdle;

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

@@ -5,6 +5,7 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.RobotBase;
+import frc.lib.littletonUtils.LoggedTunableNumber;
 import frc.robot.Constants.DriveConstants;
 import org.littletonrobotics.junction.Logger;
 
@@ -24,9 +25,11 @@ public Module(ModuleIO io, int index) {
     // Switch constants based on mode (the physics simulator is treated as a
     // separate robot with different tuning)
     if (RobotBase.isReal()) {
-      m_driveFeedforward = new SimpleMotorFeedforward(0.22810, 0.13319);
-      m_io.setDrivePID(2.0, 0.0, 0.0);
-      m_io.setTurnPID(300.0, 0.0, 0.0);
+      m_driveFeedforward = new SimpleMotorFeedforward(0.22810, 0.23319);
+      m_io.setDrivePID(
+          DriveConstants.kDriveP.get(), DriveConstants.kDriveI.get(), DriveConstants.kDriveI.get());
+      m_io.setTurnPID(
+          DriveConstants.kTurnP.get(), DriveConstants.kTurnI.get(), DriveConstants.kTurnD.get());
     } else {
       m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13);
       m_io.setDrivePID(0.1, 0.0, 0.0);
@@ -59,6 +62,30 @@ public void periodic() {
       Rotation2d angle = m_inputs.odometryTurnPositions[i];
       m_odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
     }
+
+    LoggedTunableNumber.ifChanged(
+        hashCode(),
+        () -> {
+          m_io.setDrivePID(
+              DriveConstants.kDriveP.get(),
+              DriveConstants.kDriveI.get(),
+              DriveConstants.kDriveD.get());
+        },
+        DriveConstants.kDriveP,
+        DriveConstants.kDriveI,
+        DriveConstants.kDriveD);
+
+    LoggedTunableNumber.ifChanged(
+        hashCode(),
+        () -> {
+          m_io.setTurnPID(
+              DriveConstants.kTurnP.get(),
+              DriveConstants.kTurnI.get(),
+              DriveConstants.kTurnD.get());
+        },
+        DriveConstants.kTurnP,
+        DriveConstants.kTurnI,
+        DriveConstants.kTurnD);
   }
 
   /** Runs the module with the specified setpoint state. */
@@ -102,7 +129,7 @@ public void setDriveBrakeMode(boolean enabled) {
 
   /** Returns the current turn angle of the module. */
   public Rotation2d getAngle() {
-    return m_inputs.turnPosition;
+    return m_inputs.turnAbsolutePosition;
   }
 
   /** Returns the current drive position of the module in meters. */

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

@@ -13,6 +13,8 @@
 
 package frc.robot.subsystems.drive;
 
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.Revolutions;
 
 import com.ctre.phoenix6.BaseStatusSignal;
@@ -33,13 +35,15 @@
 import com.revrobotics.spark.config.SparkBaseConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
+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.units.measure.Angle;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.Ports;
 import java.util.OptionalDouble;
 import java.util.Queue;
+import org.littletonrobotics.junction.Logger;
 
 /**
  * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO
@@ -65,14 +69,20 @@ public class ModuleIOSparkMax implements ModuleIO {
   private final Queue<Double> m_drivePositionQueue;
   private final Queue<Double> m_turnPositionQueue;
 
-  private StatusSignal<Angle> m_turnAbsolutePosition;
+  private final StatusSignal<Angle> m_turnAbsolutePosition;
 
   private final boolean m_isTurnMotorInverted = true;
   private final Rotation2d m_absoluteEncoderOffset;
 
   private final SparkBaseConfig m_driveConfig;
   private final SparkBaseConfig m_turnConfig;
 
+  private final PIDController m_turnController = new PIDController(0, 0, 0);
+
+  {
+    m_turnController.enableContinuousInput(-Math.PI, Math.PI);
+  }
+
   public ModuleIOSparkMax(int index) {
     switch (index) {
       case 0:
@@ -230,9 +240,11 @@ public void setDriveVelocity(double velocityRadPerSec, double feedforward) {
 
   @Override
   public void setTurnPosition(Rotation2d position) {
-    m_turnSparkMax
-        .getClosedLoopController()
-        .setReference(position.getMeasure().in(Revolutions), ControlType.kPosition);
+    Logger.recordOutput("Setpoint/positionTurn", position.getDegrees());
+    Logger.recordOutput("Setpoint/CancoderAngle", m_turnAbsolutePosition.getValue().in(Degrees));
+    m_turnSparkMax.setVoltage(
+        m_turnController.calculate(
+            m_turnAbsolutePosition.getValue().in(Radians), position.getRadians()));
   }
 
   @Override
@@ -270,9 +282,7 @@ public void setDrivePID(double kP, double kI, double kD) {
 
   @Override
   public void setTurnPID(double kP, double kI, double kD) {
-    m_turnConfig.closedLoop.pid(kP, kI, kD);
-    m_turnSparkMax.configure(
-        m_turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
+    m_turnController.setPID(kP, kI, kD);
   }
 
   @Override

src/main/java/frc/robot/subsystems/indexer/Indexer.java

@@ -67,4 +67,8 @@ public void updateState(IndexerState state) {
   public IndexerState getIndexerState() {
     return m_profiles.getCurrentProfile();
   }
+
+  public boolean indexerHasGamePiece() {
+    return m_inputs.hasPiece;
+  }
 }

src/main/java/frc/robot/subsystems/indexer/IndexerIO.java

@@ -3,11 +3,10 @@
 import org.littletonrobotics.junction.AutoLog;
 
 public interface IndexerIO {
-  // TODO: make sim better
   @AutoLog
   public class IndexerInputs {
     double voltage;
-    double velocityRPM;
+    double velocityRPS;
     boolean hasPiece;
     boolean photoelectric1Raw;
     boolean photoelectric2Raw;

src/main/java/frc/robot/subsystems/indexer/IndexerIONeo.java

@@ -28,7 +28,7 @@ public IndexerIONeo(int motorPort, int sensorPort1, int sensorPort2) {
   @Override
   public void updateInputs(IndexerInputs inputs) {
     inputs.voltage = m_motor.getBusVoltage() * m_motor.getAppliedOutput();
-    inputs.velocityRPM = m_encoder.getVelocity() / 60;
+    inputs.velocityRPS = m_encoder.getVelocity() / 60;
     inputs.hasPiece = !m_sensor1.get() || !m_sensor2.get();
     inputs.current = m_motor.getOutputCurrent();
     inputs.photoelectric1Raw = m_sensor1.get();

src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java

@@ -1,7 +1,6 @@
 package frc.robot.subsystems.indexer;
 
 import edu.wpi.first.math.system.plant.LinearSystemId;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import frc.robot.Constants.IndexerConstants;
@@ -20,8 +19,8 @@ public IndexerIOSim() {
                 IndexerConstants.kDCMotor, IndexerConstants.kSimMOI, IndexerConstants.kSimGearing),
             IndexerConstants.kDCMotor);
     m_voltage = 0.0;
-    m_sensor1 = new DigitalInput(Ports.kPhotoelectric1);
-    m_sensor2 = new DigitalInput(Ports.kPhotoelectric2);
+    m_sensor1 = new DigitalInput(Ports.kPhotoElectricOne);
+    m_sensor2 = new DigitalInput(Ports.kPhotoElectricTwo);
   }
 
   public void updateInputs(IndexerInputs inputs) {
@@ -30,7 +29,7 @@ public void updateInputs(IndexerInputs inputs) {
 
     inputs.hasPiece = !m_sensor1.get() || !m_sensor2.get();
     inputs.voltage = m_voltage;
-    inputs.velocityRPM = Units.radiansPerSecondToRotationsPerMinute(m_sim.getAngularVelocityRadPerSec()) / 60;
+    inputs.velocityRPS = m_sim.getAngularVelocityRPM() / 60;
   }
 
   @Override