Comp Spindexer Subsystem

src/main/java/frc/robot/Robot.java

@@ -219,7 +219,7 @@ public Robot() {
                         MotorType.KrakenX44,
                         canivore),
                 (ROBOT_MODE == RobotMode.REAL)
-                    ? new RollerIO(10, LindexerSubsystem.getIndexerConfigs(), canivore)
+                    ? new RollerIO(10, LindexerSubsystem.getKickerConfigs(), canivore)
                     : new RollerIOSim(
                         10,
                         LindexerSubsystem.getKickerConfigs(),
@@ -257,7 +257,33 @@ public Robot() {
         // note that the climber is not instantiated here
         break;
       case COMP:
-        indexer = new SpindexerSubsystem();
+        indexer =
+            new SpindexerSubsystem(
+                canivore,
+                (ROBOT_MODE == RobotMode.REAL)
+                    ? new RollerIO(9, SpindexerSubsystem.getIndexerConfigs(), canivore)
+                    : new RollerIOSim(
+                        9,
+                        SpindexerSubsystem.getIndexerConfigs(),
+                        new DCMotorSim(
+                            LinearSystemId.createDCMotorSystem(
+                                DCMotor.getKrakenX44Foc(1), 0.003, SpindexerSubsystem.GEAR_RATIO),
+                            DCMotor.getKrakenX44Foc(1)),
+                        MotorType.KrakenX44,
+                        canivore),
+                (ROBOT_MODE == RobotMode.REAL)
+                    ? new RollerIO(10, SpindexerSubsystem.getKickerConfigs(), canivore)
+                    : new RollerIOSim(
+                        10,
+                        SpindexerSubsystem.getKickerConfigs(),
+                        new DCMotorSim(
+                            LinearSystemId.createDCMotorSystem(
+                                DCMotor.getKrakenX44Foc(1),
+                                0.00001,
+                                SpindexerSubsystem.KICKER_GEAR_RATIO),
+                            DCMotor.getKrakenX44Foc(1)),
+                        MotorType.KrakenX44,
+                        canivore));
         intake = new LintakeSubsystem();
         shooter = new TurretSubsystem();
         climber = new ClimberSubsystem(); // TODO climber

src/main/java/frc/robot/Superstructure.java

@@ -80,9 +80,6 @@ public Trigger getTrigger() {
   @AutoLogOutput(key = "Superstructure/Anti Jam Req")
   private Trigger antiJamReq;
 
-  @AutoLogOutput(key = "Superstructure/Is Full")
-  private Trigger isFull;
-
   @AutoLogOutput(key = "Superstructure/Is Empty")
   private Trigger isEmpty;
 
@@ -133,8 +130,6 @@ private void addTriggers() {
 
     antiJamReq = driver.a().or(operator.a());
 
-    isFull = new Trigger(indexer::isFull).debounce(0.5); // TODO tune
-
     isEmpty = new Trigger(indexer::isEmpty);
   }
 

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

@@ -9,11 +9,9 @@
 /** Add your docs here. */
 public interface Indexer {
 
-  public boolean isFull();
-
   public boolean isEmpty();
 
-  public boolean isPartiallyFull();
+  public boolean isNotEmpty();
 
   /** Run indexer towards shooter and kicker away from shooter */
   public Command index();

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

@@ -54,19 +54,14 @@ public LindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerI
     this.indexRollerIO = indexRollerIO;
   }
 
-  @Override
-  public boolean isFull() {
-    return firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected;
-  }
-
   @Override
   public boolean isEmpty() {
     return !firstCANRangeInputs.isDetected && !secondCANRangeInputs.isDetected;
   }
 
   @Override
-  public boolean isPartiallyFull() {
-    return !firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected;
+  public boolean isNotEmpty() {
+    return secondCANRangeInputs.isDetected;
   }
 
   @Override

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

@@ -1,61 +1,168 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
 package frc.robot.subsystems.indexer;
 
+import static edu.wpi.first.units.Units.Volts;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.*;
+import frc.robot.components.canrange.CANrangeIOInputsAutoLogged;
+import frc.robot.components.canrange.CANrangeIOReal;
+import frc.robot.components.rollers.RollerIO;
+import frc.robot.components.rollers.RollerIOInputsAutoLogged;
+import org.littletonrobotics.junction.Logger;
 
-/** Spindexer = SPINning Indexer. !! COMP !! */
+/** Spindexer = Spinning Indexer. !! COMP !! */
 public class SpindexerSubsystem extends SubsystemBase implements Indexer {
-  /** Creates a new SpindexerSubsystem. */
-  public SpindexerSubsystem() {}
 
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-  }
+  public static final double GEAR_RATIO = 2.0;
+  private CANrangeIOReal CANRangeIO;
 
-  @Override
-  public boolean isFull() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'isFull'");
+  private RollerIO indexRollerIO;
+
+  CANrangeIOInputsAutoLogged CANRangeInputs = new CANrangeIOInputsAutoLogged();
+
+  RollerIOInputsAutoLogged rollerInputs = new RollerIOInputsAutoLogged();
+
+  RollerIO kickerIO;
+  RollerIOInputsAutoLogged kickerInputs = new RollerIOInputsAutoLogged();
+
+  private SysIdRoutine indexRollerSysid =
+      new SysIdRoutine(
+          new Config(
+              null,
+              null,
+              null,
+              (state) -> Logger.recordOutput("Indexer/Roller/SysID State", state.toString())),
+          new Mechanism((volts) -> indexRollerIO.setRollerVoltage(volts.in(Volts)), null, this));
+
+  public static final double MAX_ACCELERATION = 10.0;
+  public static final double MAX_VELOCITY = 10.0;
+  public static final double KICKER_GEAR_RATIO = 2.0;
+
+  public SpindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerIO) {
+    this.kickerIO = kickerIO;
+    CANRangeIO = new CANrangeIOReal(1, canbus, 10);
+    this.indexRollerIO = indexRollerIO;
   }
 
   @Override
   public boolean isEmpty() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'isEmpty'");
+    return !CANRangeInputs.isDetected;
   }
 
   @Override
-  public boolean isPartiallyFull() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'isPartiallyFull'");
+  public boolean isNotEmpty() {
+    return CANRangeInputs.isDetected;
   }
 
   @Override
   public Command index() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'index'");
+    return this.run(
+        () -> {
+          indexRollerIO.setRollerVoltage(7);
+          kickerIO.setRollerVoltage(7);
+        });
   }
 
   @Override
-  public Command spit() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'spit'");
+  public Command kick() {
+    return this.run(
+        () -> {
+          indexRollerIO.setRollerVoltage(12);
+          kickerIO.setRollerVoltage(-7);
+        });
   }
 
   @Override
-  public Command kick() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'kick'");
+  public Command spit() {
+    return this.run(
+        () -> {
+          indexRollerIO.setRollerVoltage(-5);
+          kickerIO.setRollerVoltage(-5);
+        });
   }
 
   @Override
   public Command rest() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'rest'");
+    return this.run(
+        () -> {
+          indexRollerIO.setRollerVoltage(0.0);
+          kickerIO.setRollerVoltage(0.0);
+        });
+  }
+
+  public static TalonFXConfiguration getIndexerConfigs() {
+    TalonFXConfiguration config = new TalonFXConfiguration();
+
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+    config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+    config.Feedback.SensorToMechanismRatio = GEAR_RATIO;
+
+    config.Slot0.kS = 0;
+    config.Slot0.kG = 0;
+    config.Slot0.kV = 0;
+    config.Slot0.kP = 0;
+    config.Slot0.kD = 0;
+
+    config.CurrentLimits.StatorCurrentLimit = 80.0;
+    config.CurrentLimits.StatorCurrentLimitEnable = true;
+    config.CurrentLimits.SupplyCurrentLimit = 60.0;
+    config.CurrentLimits.SupplyCurrentLimitEnable = true;
+    config.CurrentLimits.SupplyCurrentLowerLimit = 40.0;
+    config.CurrentLimits.SupplyCurrentLowerTime = 0.25;
+
+    return config;
+  }
+
+  public static TalonFXConfiguration getKickerConfigs() {
+    TalonFXConfiguration config = new TalonFXConfiguration();
+
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+    config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+    // Converts angular motion to linear motion
+    config.Feedback.SensorToMechanismRatio = KICKER_GEAR_RATIO;
+
+    config.Slot0.kS = 0;
+    config.Slot0.kG = 0;
+    config.Slot0.kV = 0;
+    config.Slot0.kP = 0;
+    config.Slot0.kD = 0;
+
+    config.CurrentLimits.StatorCurrentLimit = 80.0;
+    config.CurrentLimits.StatorCurrentLimitEnable = true;
+    config.CurrentLimits.SupplyCurrentLimit = 60.0;
+    config.CurrentLimits.SupplyCurrentLimitEnable = true;
+    config.CurrentLimits.SupplyCurrentLowerLimit = 40.0;
+    config.CurrentLimits.SupplyCurrentLowerTime = 0.25;
+
+    return config;
+  }
+
+  @Override
+  public void periodic() {
+    CANRangeIO.updateInputs(CANRangeInputs);
+    Logger.processInputs("Indexer/First Beambreak", CANRangeInputs);
+    indexRollerIO.updateInputs(rollerInputs);
+    Logger.processInputs("Indexer/Roller", rollerInputs);
+    kickerIO.updateInputs(kickerInputs);
+    Logger.processInputs("Indexer/Kicker", kickerInputs);
+  }
+
+  public Command runRollerSysId() {
+    return Commands.sequence(
+        indexRollerSysid.quasistatic(Direction.kForward),
+        indexRollerSysid.quasistatic(Direction.kReverse),
+        indexRollerSysid.dynamic(Direction.kForward),
+        indexRollerSysid.dynamic(Direction.kReverse));
   }
 }

src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java

@@ -605,6 +605,18 @@ public Command faceHub(DoubleSupplier xVel, DoubleSupplier yVel) {
         yVel);
   }
 
+  public Command bumpAlign(DoubleSupplier xVel, DoubleSupplier yVel) {
+    return driveWithHeadingSnap(
+        () -> {
+          Translation2d robotHubVec =
+              FieldUtils.getCurrentHubTranslation().minus(getPose().getTranslation());
+          // atan2 takes y as the first arg (i think bc θ = atan(y/x) but idk)
+          return Rotation2d.fromRadians(Math.atan2(robotHubVec.getY(), robotHubVec.getX()));
+        },
+        xVel,
+        yVel);
+  }
+
   public boolean isInAutoAimTolerance(Pose2d target) {
     return isInTolerance(
         target, AutoAlign.TRANSLATION_TOLERANCE_METERS, AutoAlign.ROTATION_TOLERANCE_RADIANS);