diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 996578d..402b704 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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 lastAlliance = Optional.empty(); + @AutoLogOutput boolean haveAutosGenerated = false; + private final LoggedDashboardChooser 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 diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index d4cd0bd..37698d1 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -1,11 +1,16 @@ 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; @@ -13,6 +18,8 @@ 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)); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 00b04e8..89ab34d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/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; diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index b2a7a42..4885bfb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/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); diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java index 1b0b1f9..82910fb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java +++ b/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; diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index c1ceeaf..44ea671 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/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 hoodVoltage = hoodMotor.getMotorVoltage(); - private final StatusSignal hoodStatorCurrent = hoodMotor.getStatorCurrent(); - private final StatusSignal hoodSupplyCurrent = hoodMotor.getSupplyCurrent(); - private final StatusSignal hoodTemp = hoodMotor.getDeviceTemp(); + private final BaseStatusSignal hoodPositionRotations; + private final BaseStatusSignal hoodAngularVelocity; + private final StatusSignal hoodVoltage; + private final StatusSignal hoodStatorCurrent; + private final StatusSignal hoodSupplyCurrent; + private final StatusSignal 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; diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java index a2c0153..7aafe9e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java +++ b/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); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index fc4ba41..99bc648 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/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)); + } }