diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 402b704..c42a549 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -378,6 +378,7 @@ private void addAutos() { autoChooser.addOption("Hood Sysid", shooter.runHoodSysid()); autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId()); autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid()); + autoChooser.addOption("Kicker Sysid", indexer.runKickerSysId()); autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); haveAutosGenerated = true; System.out.println("Done generating autos"); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 37698d1..6057df0 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -42,6 +42,15 @@ public class IndexerSubsystem extends SubsystemBase { (state) -> Logger.recordOutput("Indexer/Roller/SysID State", state)), new Mechanism((volts) -> indexRollerIO.setRollerVoltage(volts.in(Volts)), null, this)); + private SysIdRoutine kickerRollerSysid = + new SysIdRoutine( + new Config( + null, + null, + null, + (state) -> Logger.recordOutput("Indexer/Kicker/SysID State", state)), + new Mechanism((volts) -> kickerIO.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; @@ -174,4 +183,12 @@ public Command runRollerSysId() { indexRollerSysid.dynamic(Direction.kForward), indexRollerSysid.dynamic(Direction.kReverse)); } + + public Command runKickerSysId() { + return Commands.sequence( + kickerRollerSysid.quasistatic(Direction.kForward), + kickerRollerSysid.quasistatic(Direction.kReverse), + kickerRollerSysid.dynamic(Direction.kForward), + kickerRollerSysid.dynamic(Direction.kReverse)); + } }