Sysid

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

@@ -100,12 +100,13 @@ public enum RobotType {
       new IndexerSubsystem(
           canivore,
           (ROBOT_TYPE == RobotType.REAL)
-              ? new RollerIO(9, IndexerSubsystem.getIndexerConfigs(), canivore) // TODO follower
+              ? new RollerIO(9, IndexerSubsystem.getIndexerConfigs(), canivore)
               : new RollerIOSim(
                   9,
                   IndexerSubsystem.getIndexerConfigs(),
                   new DCMotorSim(
-                      LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX44Foc(1), 1, 1),
+                      LinearSystemId.createDCMotorSystem(
+                          DCMotor.getKrakenX44Foc(1), 0.003, IndexerSubsystem.GEAR_RATIO),
                       DCMotor.getKrakenX44Foc(1)),
                   MotorType.KrakenX44,
                   canivore),
@@ -133,13 +134,14 @@ public enum RobotType {
               : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore));
   private final IntakeSubsystem intake =
       new IntakeSubsystem(
-          ROBOT_TYPE == RobotType.REAL
+          (ROBOT_TYPE == RobotType.REAL)
               ? new RollerIO(8, IntakeSubsystem.getIntakeConfig(), canivore)
               : new RollerIOSim(
                   8,
                   IntakeSubsystem.getIntakeConfig(),
                   new DCMotorSim(
-                      LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX44Foc(1), 1, 1),
+                      LinearSystemId.createDCMotorSystem(
+                          DCMotor.getKrakenX44Foc(1), 0.001, IntakeSubsystem.GEAR_RATIO),
                       DCMotor.getKrakenX44Foc(1)),
                   MotorType.KrakenX44,
                   canivore));
@@ -160,7 +162,9 @@ public enum RobotType {
 
   private final Autos autos;
   private Optional<Alliance> lastAlliance = Optional.empty();
+
   @AutoLogOutput boolean haveAutosGenerated = false;
+
   private final LoggedDashboardChooser<Command> autoChooser = new LoggedDashboardChooser<>("Autos");
 
   // Logged mechanisms
@@ -369,7 +373,14 @@ private void addAutos() {
     System.out.println("------- Regenerating Autos");
     System.out.println(
         "Regenerating Autos on " + DriverStation.getAlliance().map((a) -> a.toString()));
+
+    // Sysid Autos
+    autoChooser.addOption("Hood Sysid", shooter.runHoodSysid());
+    autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId());
+    autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid());
+    autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid());
     haveAutosGenerated = true;
+    System.out.println("Done generating autos");
   }
 
   @Override

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

@@ -1,18 +1,25 @@
 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;
 
 public class IndexerSubsystem extends SubsystemBase {
+
+  public static final double GEAR_RATIO = 2.0;
   private CANrangeIOReal firstCANRangeIO;
   private CANrangeIOReal secondCANRangeIO;
 
@@ -26,6 +33,15 @@ public class IndexerSubsystem extends SubsystemBase {
   RollerIO kickerIO;
   RollerIOInputsAutoLogged kickerInputs = new RollerIOInputsAutoLogged();
 
+  private SysIdRoutine indexRollerSysid =
+      new SysIdRoutine(
+          new Config(
+              null,
+              null,
+              null,
+              (state) -> Logger.recordOutput("Indexer/Roller/SysID State", state)),
+          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;
@@ -95,8 +111,7 @@ public static TalonFXConfiguration getIndexerConfigs() {
 
     config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
 
-    // Converts angular motion to linear motion
-    config.Feedback.SensorToMechanismRatio = 1;
+    config.Feedback.SensorToMechanismRatio = GEAR_RATIO;
 
     config.Slot0.kS = 0;
     config.Slot0.kG = 0;
@@ -151,4 +166,12 @@ public void periodic() {
     kickerIO.updateInputs(kickerInputs);
     Logger.processInputs("Intake/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/intake/IntakeSubsystem.java

@@ -1,19 +1,30 @@
 package frc.robot.subsystems.intake;
 
+import static edu.wpi.first.units.Units.Volts;
+
 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.rollers.RollerIO;
 import frc.robot.components.rollers.RollerIOInputsAutoLogged;
 import org.littletonrobotics.junction.Logger;
 
 public class IntakeSubsystem extends SubsystemBase {
+  public static final double GEAR_RATIO = 2.0;
 
   private RollerIO io;
   private RollerIOInputsAutoLogged inputs = new RollerIOInputsAutoLogged();
 
+  private SysIdRoutine intakeRollerSysid =
+      new SysIdRoutine(
+          new Config(null, null, null, (state) -> Logger.recordOutput("Intake/SysID State", state)),
+          new Mechanism((volts) -> io.setRollerVoltage(volts.in(Volts)), null, this));
+
   public IntakeSubsystem(RollerIO io) {
     this.io = io;
   }
@@ -36,11 +47,21 @@ public Command rest() {
     return this.run(() -> io.setRollerVoltage(0));
   }
 
+  public Command runRollerSysid() {
+    return Commands.sequence(
+        intakeRollerSysid.quasistatic(Direction.kForward),
+        intakeRollerSysid.quasistatic(Direction.kReverse),
+        intakeRollerSysid.dynamic(Direction.kForward),
+        intakeRollerSysid.dynamic(Direction.kReverse));
+  }
+
   public static TalonFXConfiguration getIntakeConfig() {
     TalonFXConfiguration config = new TalonFXConfiguration();
     config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
     config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
 
+    config.Feedback.SensorToMechanismRatio = GEAR_RATIO;
+
     config.Slot0.kS = 0.24;
     config.Slot0.kV = 0.6;
     config.Slot0.kP = 110.0;

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

@@ -62,8 +62,8 @@ public static class FlywheelIOInputs {
   // todo: tune acceleration
 
   public FlywheelIO(TalonFXConfiguration config, CANBus canbus) {
-    flywheelLeader = new TalonFX(10, canbus);
-    flywheelFollower = new TalonFX(11, canbus);
+    flywheelLeader = new TalonFX(12, canbus);
+    flywheelFollower = new TalonFX(13, canbus);
 
     flywheelLeader.getConfigurator().apply(config);
     flywheelFollower.getConfigurator().apply(config);

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

@@ -25,7 +25,7 @@ public class FlywheelIOSim extends FlywheelIO {
   DCMotorSim physicsSim =
       new DCMotorSim(
           LinearSystemId.createDCMotorSystem(
-              DCMotor.getKrakenX60Foc(2), 0.01, ShooterSubsystem.GEAR_RATIO),
+              DCMotor.getKrakenX60Foc(2), 0.0136, ShooterSubsystem.FLYWHEEL_GEAR_RATIO),
           DCMotor.getKrakenX60Foc(2));
 
   private final double simLoopPeriod = 0.002;

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

@@ -37,20 +37,27 @@ public static class HoodIOInputs {
 
   protected TalonFX hoodMotor;
 
-  private final BaseStatusSignal hoodPositionRotations = hoodMotor.getPosition();
-  private final BaseStatusSignal hoodAngularVelocity = hoodMotor.getVelocity();
-  private final StatusSignal<Voltage> hoodVoltage = hoodMotor.getMotorVoltage();
-  private final StatusSignal<Current> hoodStatorCurrent = hoodMotor.getStatorCurrent();
-  private final StatusSignal<Current> hoodSupplyCurrent = hoodMotor.getSupplyCurrent();
-  private final StatusSignal<Temperature> hoodTemp = hoodMotor.getDeviceTemp();
+  private final BaseStatusSignal hoodPositionRotations;
+  private final BaseStatusSignal hoodAngularVelocity;
+  private final StatusSignal<Voltage> hoodVoltage;
+  private final StatusSignal<Current> hoodStatorCurrent;
+  private final StatusSignal<Current> hoodSupplyCurrent;
+  private final StatusSignal<Temperature> hoodTemp;
   private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true);
   private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true);
   private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true);
 
   public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) {
-    hoodMotor = new TalonFX(12, canbus);
+    hoodMotor = new TalonFX(11, canbus);
     hoodMotor.getConfigurator().apply(HoodIO.getHoodConfiguration());
 
+    hoodPositionRotations = hoodMotor.getPosition();
+    hoodAngularVelocity = hoodMotor.getVelocity();
+    hoodVoltage = hoodMotor.getMotorVoltage();
+    hoodStatorCurrent = hoodMotor.getStatorCurrent();
+    hoodSupplyCurrent = hoodMotor.getSupplyCurrent();
+    hoodTemp = hoodMotor.getDeviceTemp();
+
     BaseStatusSignal.setUpdateFrequencyForAll(
         50.0,
         hoodPositionRotations,
@@ -73,7 +80,7 @@ public static TalonFXConfiguration getHoodConfiguration() {
 
     config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
 
-    config.Feedback.SensorToMechanismRatio = ShooterSubsystem.GEAR_RATIO;
+    config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO;
 
     config.Slot0.GravityType = GravityTypeValue.Elevator_Static;
 

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

@@ -17,7 +17,7 @@ public class HoodIOSim extends HoodIO {
   private final DCMotorSim hoodPhysicsSim =
       new DCMotorSim(
           LinearSystemId.createDCMotorSystem(
-              DCMotor.getKrakenX44Foc(1), 0.01, ShooterSubsystem.GEAR_RATIO),
+              DCMotor.getKrakenX44Foc(1), 0.01, ShooterSubsystem.HOOD_GEAR_RATIO),
           DCMotor.getKrakenX44Foc(1));
 
   // will get updated when i get specs
@@ -47,9 +47,9 @@ public HoodIOSim(CANBus canbus) {
               // rotor position stuff added later when i have access to onshape
 
               hoodMotorSim.setRawRotorPosition(
-                  hoodPhysicsSim.getAngularPositionRad() * (ShooterSubsystem.GEAR_RATIO));
+                  hoodPhysicsSim.getAngularPositionRad() * (ShooterSubsystem.HOOD_GEAR_RATIO));
               hoodMotorSim.setRotorVelocity(
-                  hoodPhysicsSim.getAngularVelocityRPM() / 60.0 * ShooterSubsystem.GEAR_RATIO);
+                  hoodPhysicsSim.getAngularVelocityRPM() / 60.0 * ShooterSubsystem.HOOD_GEAR_RATIO);
             });
     simNotifier.startPeriodic(simLoopPeriod);
   }

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

@@ -4,22 +4,48 @@
 
 package frc.robot.subsystems.shooter;
 
+import static edu.wpi.first.units.Units.Volts;
+
 import com.google.common.base.Supplier;
 import edu.wpi.first.math.geometry.Rotation2d;
 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.Config;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism;
 import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
 
 public class ShooterSubsystem extends SubsystemBase {
+  public static double HOOD_GEAR_RATIO = 147.0 / 13.0;
+  public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40);
+  public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(0);
+
+  public static double FLYWHEEL_GEAR_RATIO = 28.0 / 24.0;
+
   HoodIO hoodIO;
   HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged();
 
-  public static double GEAR_RATIO = 147.0 / 13.0;
-
   FlywheelIO flywheelIO;
   FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged();
 
+  private SysIdRoutine hoodSysid =
+      new SysIdRoutine(
+          new Config(
+              null, null, null, (state) -> Logger.recordOutput("Shooter/Hood/SysID State", state)),
+          new Mechanism((voltage) -> hoodIO.setHoodVoltage(voltage.in(Volts)), null, this));
+
+  private SysIdRoutine flywheelSysid =
+      new SysIdRoutine(
+          new Config(
+              null,
+              null,
+              null,
+              (state) -> Logger.recordOutput("Shooter/Flywheel/SysID State", state)),
+          new Mechanism((voltage) -> flywheelIO.setFlywheelVoltage(voltage.in(Volts)), null, this));
+
   /** Creates a new HoodSubsystem. */
   public ShooterSubsystem(HoodIO hoodIO, FlywheelIO flywheelIO) {
     this.hoodIO = hoodIO;
@@ -46,4 +72,40 @@ public void periodic() {
     flywheelIO.updateInputs(flywheelInputs);
     Logger.processInputs("Shooter/Flywheel", flywheelInputs);
   }
+
+  public Command runHoodSysid() {
+    return Commands.sequence(
+        hoodSysid
+            .quasistatic(Direction.kForward)
+            .until(
+                () ->
+                    hoodInputs.hoodPositionRotations.getDegrees()
+                        > (HOOD_MAX_ROTATION.getDegrees() - 5)), // Stop before endstop
+        hoodSysid
+            .quasistatic(Direction.kReverse)
+            .until(
+                () ->
+                    hoodInputs.hoodPositionRotations.getDegrees()
+                        < (HOOD_MIN_ROTATION.getDegrees() + 5)),
+        hoodSysid
+            .dynamic(Direction.kForward)
+            .until(
+                () ->
+                    hoodInputs.hoodPositionRotations.getDegrees()
+                        > (HOOD_MAX_ROTATION.getDegrees() - 5)),
+        hoodSysid
+            .dynamic(Direction.kReverse)
+            .until(
+                () ->
+                    hoodInputs.hoodPositionRotations.getDegrees()
+                        < (HOOD_MIN_ROTATION.getDegrees() + 5)));
+  }
+
+  public Command runFlywheelSysid() {
+    return Commands.sequence(
+        flywheelSysid.quasistatic(Direction.kForward),
+        flywheelSysid.quasistatic(Direction.kReverse),
+        flywheelSysid.dynamic(Direction.kForward),
+        flywheelSysid.dynamic(Direction.kReverse));
+  }
 }