diff --git a/ci-scripts/check_constants.py b/ci-scripts/check_constants.py index 46db485d..a338b9ca 100644 --- a/ci-scripts/check_constants.py +++ b/ci-scripts/check_constants.py @@ -25,6 +25,7 @@ "ShooterConstants.json": "frc.robot.constants.ShooterConstants", "HoodConstants.json": "frc.robot.constants.HoodConstants", "ClimberConstants.json": "frc.robot.constants.ClimberConstants", + "TransferRollerConstants.json": "frc.robot.constants.TransferRollerConstants", "ShotMaps.json": "frc.robot.constants.ShotMaps", "RedFieldLocations.json": "frc.robot.constants.FieldLocationInstance", "BlueFieldLocations.json": "frc.robot.constants.FieldLocationInstance", diff --git a/elastic_layouts/tuning.json b/elastic_layouts/tuning.json index 5832c024..4e10bb74 100644 --- a/elastic_layouts/tuning.json +++ b/elastic_layouts/tuning.json @@ -2380,6 +2380,290 @@ } ] } + }, + { + "name": "Transfer Roller Tuning", + "grid_layout": { + "layouts": [ + { + "title": "PIDGains", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "LEFT" + }, + "children": [ + { + "title": "kP", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/PIDGains/kP", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kI", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/PIDGains/kI", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kD", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/PIDGains/kD", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "FeedForwardGains", + "x": 256.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "LEFT" + }, + "children": [ + { + "title": "kA", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/PIDGains/kA", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kG", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/PIDGains/kG", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kS", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/PIDGains/kS", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kV", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/PIDGains/kV", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "MotionProfile", + "x": 896.0, + "y": 0.0, + "width": 384.0, + "height": 384.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "MaxAcceleration_Rotation per Second per Second", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/MotionProfile/MaxAcceleration_Rotation per Second per Second", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "MaxJerk_Rotation per Second per Second per Second", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/MotionProfile/MaxJerk_Rotation per Second per Second per Second", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "MaxVelocity_Rotation per Second", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/MotionProfile/MaxVelocity_Rotation per Second", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "expoKA_Volt per Rotation per Second per Second", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/MotionProfile/expoKA_Volt per Rotation per Second per Second", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "expoKV_Volt per Rotation per Second", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/MotionProfile/expoKV_Volt per Rotation per Second", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "MotorTuning", + "x": 512.0, + "y": 0.0, + "width": 384.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "CurrentTuning_Amp", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/CurrentTuning_Amp", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "VelocityTarget_Rotation per Minute", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/VelocityTarget_Rotation per Minute", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "VoltageTuning_Volt", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "TunableNumbers/TransferRoller/MotorTuning/VoltageTuning_Volt", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + } + ], + "containers": [ + { + "title": "TransferRoller Test Mode Selector", + "x": 0.0, + "y": 256.0, + "width": 896.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/TransferRoller Test Mode Selector", + "period": 0.06, + "sort_options": false + } + } + ] + } } ] } \ No newline at end of file diff --git a/src/main/deploy/constants/comp/CANBusAssignment.json b/src/main/deploy/constants/comp/CANBusAssignment.json index 5fb4ae48..ac03bd6d 100644 --- a/src/main/deploy/constants/comp/CANBusAssignment.json +++ b/src/main/deploy/constants/comp/CANBusAssignment.json @@ -22,5 +22,6 @@ "intakeRollersFollowerMotorId": 21, "shooterLeaderId": 22, "shooterFollowerId": 23, - "climberKrakenId": 24 + "climberKrakenId": 24, + "transferRollerKrakenId": 25 } diff --git a/src/main/deploy/constants/comp/FeatureFlags.json b/src/main/deploy/constants/comp/FeatureFlags.json index 88354cec..d2ed928f 100644 --- a/src/main/deploy/constants/comp/FeatureFlags.json +++ b/src/main/deploy/constants/comp/FeatureFlags.json @@ -9,6 +9,7 @@ "runIntake": true, "runHood": true, "runClimber": true, + "runTransferRoller": true, "useHomingSwitch": false, "useTuningServer": true, "pretendCamerasAreMobile": false diff --git a/src/main/deploy/constants/comp/TransferRollerConstants.json b/src/main/deploy/constants/comp/TransferRollerConstants.json new file mode 100644 index 00000000..dc767518 --- /dev/null +++ b/src/main/deploy/constants/comp/TransferRollerConstants.json @@ -0,0 +1,72 @@ +{ + "transferRollerReduction": 1.0, + "velocityFilterTime": { + "value": 0.05, + "unit": "Seconds" + }, + "transferRollerGains": { + "kP": 10.0, + "kI": 7.0, + "kD": 0.0, + "kS": 2.0, + "kG": 0.0, + "kV": 0.1, + "kA": 0.0 + }, + "transferRollerMotionProfileConfig": { + "isMutable": false, + "maxVelocity": { + "value": 0.0, + "unit": "Rotation per Second" + }, + "maxAcceleration": { + "value": 3000.0, + "unit": "Rotation per Second per Second" + }, + "maxJerk": { + "value": 0.0, + "unit": "Rotation per Second per Second per Second" + }, + "expoKV": { + "dividend": { + "value": 0.0, + "unit": "Volt" + }, + "divisor": { + "value": 1.0, + "unit": "Rotation per Second" + } + }, + "expoKA": { + "dividend": { + "value": 0.0, + "unit": "Volt" + }, + "divisor": { + "value": 1.0, + "unit": "Rotation per Second per Second" + } + } + }, + "transferRollerSupplyCurrentLimit": { + "value": 60.0, + "unit": "Amp" + }, + "transferRollerStatorCurrentLimit": { + "value": 40.0, + "unit": "Amp" + }, + "simTransferRollerMOI": { + "value": 2.5E-4, + "unit": "Kilogram-Meter per Second-Meter per Radian per Second" + }, + "transferRollerSpinningVelocity": { + "value": 2000.0, + "unit": "Rotation per Minute" + }, + "transferRollerDeJamVelocity": { + "value": -2000.0, + "unit": "Rotation per Minute" + }, + "transferRollerMotorDirection": "CounterClockwise_Positive" +} diff --git a/src/main/deploy/constants/test_drivebase/CANBusAssignment.json b/src/main/deploy/constants/test_drivebase/CANBusAssignment.json index fa8bac47..20ff92b2 100644 --- a/src/main/deploy/constants/test_drivebase/CANBusAssignment.json +++ b/src/main/deploy/constants/test_drivebase/CANBusAssignment.json @@ -22,5 +22,6 @@ "intakeRollersFollowerMotorId": 21, "shooterLeaderId": 22, "shooterFollowerId": 23, - "climberKrakenId": 24 + "climberKrakenId": 24, + "transferRollerKrakenId": 25 } diff --git a/src/main/deploy/constants/test_drivebase/FeatureFlags.json b/src/main/deploy/constants/test_drivebase/FeatureFlags.json index 3727f376..9e44ff10 100644 --- a/src/main/deploy/constants/test_drivebase/FeatureFlags.json +++ b/src/main/deploy/constants/test_drivebase/FeatureFlags.json @@ -9,6 +9,7 @@ "runIntake": false, "runHood": false, "runClimber": false, + "runTransferRoller": false, "useHomingSwitch": false, "useTuningServer": true, "pretendCamerasAreMobile": true diff --git a/src/main/java/frc/robot/CoordinationLayer.java b/src/main/java/frc/robot/CoordinationLayer.java index f88ed90f..85191711 100644 --- a/src/main/java/frc/robot/CoordinationLayer.java +++ b/src/main/java/frc/robot/CoordinationLayer.java @@ -4,6 +4,7 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Seconds; import coppercore.controls.state_machine.State; @@ -50,6 +51,7 @@ import frc.robot.subsystems.indexer.IndexerSubsystem; import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.transferroller.TransferRollerSubsystem; import frc.robot.subsystems.turret.TurretSubsystem; import frc.robot.util.AllianceUtil; import frc.robot.util.OptionalUtil; @@ -77,6 +79,7 @@ public class CoordinationLayer { private Optional vision = Optional.empty(); private Optional hopper = Optional.empty(); private Optional indexer = Optional.empty(); + private Optional transferRoller = Optional.empty(); private Optional turret = Optional.empty(); private Optional shooter = Optional.empty(); private Optional hood = Optional.empty(); @@ -562,6 +565,11 @@ public void setIndexer(IndexerSubsystem indexer) { this.indexer = Optional.of(indexer); } + public void setTransferRoller(TransferRollerSubsystem transferRoller) { + checkForDuplicateSubsystem(this.transferRoller, "TransferRoller"); + this.transferRoller = Optional.of(transferRoller); + } + public void setIntake(IntakeSubsystem intake) { checkForDuplicateSubsystem(this.intake, "Intake"); this.intake = Optional.of(intake); @@ -753,9 +761,15 @@ public void coordinateRobotActions() { hopper -> hopper.setTargetVelocity(JsonConstants.hopperConstants.indexingVelocity)); indexer.ifPresent( indexer -> indexer.setTargetVelocity(JsonConstants.indexerConstants.indexingVelocity)); + transferRoller.ifPresent( + transferRoller -> + transferRoller.setTargetVelocity( + JsonConstants.transferRollerConstants.transferRollerSpinningVelocity)); } else { hopper.ifPresent(hopper -> hopper.setTargetVelocity(RPM.zero())); indexer.ifPresent(indexer -> indexer.setTargetVelocity(RPM.zero())); + transferRoller.ifPresent( + transferRoller -> transferRoller.setTargetVelocity(RadiansPerSecond.zero())); } // Aim for a shot based on the current autonomy level diff --git a/src/main/java/frc/robot/InitSubsystems.java b/src/main/java/frc/robot/InitSubsystems.java index 8ce33a33..18ac01a7 100644 --- a/src/main/java/frc/robot/InitSubsystems.java +++ b/src/main/java/frc/robot/InitSubsystems.java @@ -29,6 +29,7 @@ import frc.robot.subsystems.indexer.IndexerSubsystem; import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.transferroller.TransferRollerSubsystem; import frc.robot.subsystems.turret.TurretSubsystem; import frc.robot.util.io.dio_switch.DigitalInputIOCANdi; import frc.robot.util.io.dio_switch.DigitalInputIOCANdiSimNT; @@ -277,6 +278,32 @@ public static IndexerSubsystem initIndexerSubsystem() { } } + public static TransferRollerSubsystem initTransferRollerSubsystem() { + switch (Constants.currentMode) { + case REAL: + // Real robot, instantiate hardware IO implementations + return new TransferRollerSubsystem( + MotorIOTalonFX.newLeader( + JsonConstants.transferRollerConstants.buildMechanismConfig(), + JsonConstants.transferRollerConstants.buildTalonFXConfigs(), + JsonConstants.robotInfo.nonFireControllingRefreshRates)); + case SIM: + // Sim robot, instantiate physics sim IO implementations + MechanismConfig config = JsonConstants.transferRollerConstants.buildMechanismConfig(); + return new TransferRollerSubsystem( + MotorIOTalonFXSim.newLeader( + config, + JsonConstants.transferRollerConstants.buildTalonFXConfigs(), + JsonConstants.transferRollerConstants.buildTransferRollerSim()) + .withMotorType(MotorType.KrakenX44)); + case REPLAY: + // Replayed robot, disable IO implementations + return new TransferRollerSubsystem(new MotorIOReplay()); + default: + throw new UnsupportedOperationException("Unsupported mode " + Constants.currentMode); + } + } + public static TurretSubsystem initTurretSubsystem( DependencyOrderedExecutor dependencyOrderedExecutor) { switch (Constants.currentMode) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7b6822c4..b87d43a2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,7 @@ import frc.robot.subsystems.indexer.IndexerSubsystem; import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.transferroller.TransferRollerSubsystem; import frc.robot.subsystems.turret.TurretSubsystem; import java.util.Optional; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -97,6 +98,11 @@ public RobotContainer() { coordinationLayer.setIndexer(indexer); } + if (JsonConstants.featureFlags.runTransferRoller) { + TransferRollerSubsystem transferRoller = InitSubsystems.initTransferRollerSubsystem(); + coordinationLayer.setTransferRoller(transferRoller); + } + if (JsonConstants.featureFlags.runIntake) { IntakeSubsystem intakeSubsystem = InitSubsystems.initIntakeSubsystem(); coordinationLayer.setIntake(intakeSubsystem); diff --git a/src/main/java/frc/robot/constants/CANBusAssignment.java b/src/main/java/frc/robot/constants/CANBusAssignment.java index b438af97..5a9df8e9 100644 --- a/src/main/java/frc/robot/constants/CANBusAssignment.java +++ b/src/main/java/frc/robot/constants/CANBusAssignment.java @@ -40,5 +40,7 @@ public class CANBusAssignment { public final Integer shooterLeaderId = 22; public final Integer shooterFollowerId = 23; - public final Integer climberKrakenId = 24; // TODO: Verify this ID + public final Integer climberKrakenId = 24; + + public final Integer transferRollerKrakenId = 25; } diff --git a/src/main/java/frc/robot/constants/FeatureFlags.java b/src/main/java/frc/robot/constants/FeatureFlags.java index a3923e35..f5d27726 100644 --- a/src/main/java/frc/robot/constants/FeatureFlags.java +++ b/src/main/java/frc/robot/constants/FeatureFlags.java @@ -19,6 +19,7 @@ public class FeatureFlags { public final Boolean runIntake = false; public final Boolean runHood = false; public final Boolean runClimber = true; + public final Boolean runTransferRoller = false; public final Boolean useHomingSwitch = false; public final Boolean useTuningServer = false; public final Boolean pretendCamerasAreMobile = false; @@ -37,6 +38,7 @@ public void logFlags() { System.out.println(" - runIntake: " + runIntake); System.out.println(" - runHood: " + runHood); System.out.println(" - runClimber: " + runClimber); + System.out.println(" - runTransferRoller: " + runTransferRoller); System.out.println(" - useHomingSwitch: " + useHomingSwitch); System.out.println(" - useTuningServer: " + useTuningServer); System.out.println(" - pretendCamerasAreMobile: " + pretendCamerasAreMobile); @@ -51,6 +53,7 @@ public void logFlags() { Logger.recordOutput("FeatureFlags/runIntake", runIntake); Logger.recordOutput("FeatureFlags/runHood", runHood); Logger.recordOutput("FeatureFlags/runClimber", runClimber); + Logger.recordOutput("FeatureFlags/runTransferRoller", runTransferRoller); Logger.recordOutput("FeatureFlags/useHomingSwitch", useHomingSwitch); Logger.recordOutput("FeatureFlags/useTuningServer", useTuningServer); Logger.recordOutput("FeatureFlags/pretendCamerasAreMobile", pretendCamerasAreMobile); diff --git a/src/main/java/frc/robot/constants/JsonConstants.java b/src/main/java/frc/robot/constants/JsonConstants.java index df1c327f..576cb999 100644 --- a/src/main/java/frc/robot/constants/JsonConstants.java +++ b/src/main/java/frc/robot/constants/JsonConstants.java @@ -78,6 +78,8 @@ public static void loadConstants() { shooterConstants = jsonHandler.getObject(new ShooterConstants(), "ShooterConstants.json"); hoodConstants = jsonHandler.getObject(new HoodConstants(), "HoodConstants.json"); climberConstants = jsonHandler.getObject(new ClimberConstants(), "ClimberConstants.json"); + transferRollerConstants = + jsonHandler.getObject(new TransferRollerConstants(), "TransferRollerConstants.json"); shotMaps = jsonHandler.getObject(new ShotMaps(), "ShotMaps.json"); redFieldLocations = jsonHandler.getObject(new FieldLocationInstance(), "RedFieldLocations.json"); @@ -96,6 +98,7 @@ public static void loadConstants() { jsonHandler.addRoute("/shooter", shooterConstants); jsonHandler.addRoute("/drive", driveConstants); jsonHandler.addRoute("/hood", hoodConstants); + jsonHandler.addRoute("/transferroller", transferRollerConstants); jsonHandler.addRoute("/intake", intakeConstants); jsonHandler.addRoute("/climber", climberConstants); jsonHandler.addRoute("/vision", visionConstants); @@ -136,6 +139,7 @@ public static void loadConstants() { public static IntakeConstants intakeConstants; public static ShooterConstants shooterConstants; public static HoodConstants hoodConstants; + public static TransferRollerConstants transferRollerConstants; public static ShotMaps shotMaps; public static FieldLocationInstance redFieldLocations; public static FieldLocationInstance blueFieldLocations; diff --git a/src/main/java/frc/robot/constants/TransferRollerConstants.java b/src/main/java/frc/robot/constants/TransferRollerConstants.java new file mode 100644 index 00000000..9bea3aef --- /dev/null +++ b/src/main/java/frc/robot/constants/TransferRollerConstants.java @@ -0,0 +1,98 @@ +package frc.robot.constants; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import coppercore.wpilib_interface.subsystems.configs.CANDeviceID; +import coppercore.wpilib_interface.subsystems.configs.MechanismConfig; +import coppercore.wpilib_interface.subsystems.configs.MechanismConfig.GravityFeedforwardType; +import coppercore.wpilib_interface.subsystems.motors.profile.MotionProfileConfig; +import coppercore.wpilib_interface.subsystems.sim.CoppercoreSimAdapter; +import coppercore.wpilib_interface.subsystems.sim.DCMotorSimAdapter; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.MomentOfInertia; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.util.PIDGains; + +public class TransferRollerConstants { + + public final Double transferRollerReduction = 1.0; // TODO: find actual value + + public final Time velocityFilterTime = Seconds.of(0.05); + + public PIDGains transferRollerGains = PIDGains.kPID(10.0, 5.0, 0.0); + + // The important values here are maxAcceleration and maxJerk because it uses + // a profiled velocity request + public MotionProfileConfig transferRollerMotionProfileConfig = + MotionProfileConfig.immutable( + RotationsPerSecond.zero(), + RotationsPerSecondPerSecond.of(3000.0), + RotationsPerSecondPerSecond.of(1000.0).div(Seconds.of(1.0)), + Volts.zero().div(RotationsPerSecond.of(1)), + Volts.zero().div(RotationsPerSecondPerSecond.of(1))); + + public MechanismConfig buildMechanismConfig() { + return MechanismConfig.builder() + .withName("TransferRoller") + .withEncoderToMechanismRatio(transferRollerReduction) + .withMotorToEncoderRatio(1.0) + .withGravityFeedforwardType(GravityFeedforwardType.STATIC_ELEVATOR) + .withLeadMotorId( + new CANDeviceID( + JsonConstants.robotInfo.CANBus, + JsonConstants.canBusAssignment.transferRollerKrakenId)) + .build(); + } + + // TODO: Find actual values + public final Current transferRollerSupplyCurrentLimit = Amps.of(60.0); + public final Current transferRollerStatorCurrentLimit = Amps.of(40.0); + public final MomentOfInertia simTransferRollerMOI = KilogramSquareMeters.of(0.00025); + + public final AngularVelocity transferRollerSpinningVelocity = RPM.of(2000); + public final AngularVelocity transferRollerDeJamVelocity = RPM.of(-2000); + + public final InvertedValue transferRollerMotorDirection = InvertedValue.CounterClockwise_Positive; + + public TalonFXConfiguration buildTalonFXConfigs() { + return new TalonFXConfiguration() + .withSlot0(transferRollerGains.toSlot0Config()) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(transferRollerSupplyCurrentLimit) + .withSupplyCurrentLimitEnable(true) + .withStatorCurrentLimit(transferRollerStatorCurrentLimit) + .withStatorCurrentLimitEnable(true)) + .withMotorOutput(new MotorOutputConfigs().withInverted(transferRollerMotorDirection)) + .withMotionMagic(transferRollerMotionProfileConfig.asMotionMagicConfigs()) + .withFeedback(new FeedbackConfigs().withVelocityFilterTimeConstant(velocityFilterTime)); + } + + public CoppercoreSimAdapter buildTransferRollerSim() { + return new DCMotorSimAdapter( + buildMechanismConfig(), + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), + simTransferRollerMOI.in(KilogramSquareMeters), + 1 / transferRollerReduction), + DCMotor.getKrakenX44Foc(1), + 0.0, + 0.0)); + } +} diff --git a/src/main/java/frc/robot/subsystems/transferroller/TestMode.java b/src/main/java/frc/robot/subsystems/transferroller/TestMode.java new file mode 100644 index 00000000..9a884da3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/transferroller/TestMode.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.transferroller; + +import frc.robot.util.TestModeDescription; + +public enum TestMode implements TestModeDescription { + // Intake Roller test modes + TransferRollerVoltageTuning("TransferRoller Voltage Open Loop Tuning"), + TransferRollerCurrentTuning("TransferRoller Current Open Loop Tuning"), + TransferRollerClosedLoopTuning("TransferRoller Closed Loop Tuning"), + TransferRollerPhoenixTuning("TransferRoller Phoenix Tuning (no-op)"), + // Miscellaneous + None; // No test mode selected + + private final String description; + + TestMode(String description) { + this.description = description; + } + + TestMode() { + this.description = name(); + } + + @Override + public String getDescription() { + return this.description; + } +} diff --git a/src/main/java/frc/robot/subsystems/transferroller/TransferRollerState.java b/src/main/java/frc/robot/subsystems/transferroller/TransferRollerState.java new file mode 100644 index 00000000..fb8dffc3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/transferroller/TransferRollerState.java @@ -0,0 +1,47 @@ +package frc.robot.subsystems.transferroller; + +import coppercore.controls.state_machine.State; +import coppercore.controls.state_machine.StateMachine; + +public abstract class TransferRollerState extends State { + + public static class IdleState extends TransferRollerState { + @Override + public void periodic( + StateMachine stateMachine, + TransferRollerSubsystem transferRoller) { + transferRoller.coast(); + } + } + + /** + * TestModeState calls testPeriodic and does nothing else, to allow for the subsystem's + * testPeriodic method to take action when in test mode without conflict. + */ + public static class TestModeState extends TransferRollerState { + @Override + public void periodic( + StateMachine stateMachine, + TransferRollerSubsystem transferRoller) { + transferRoller.testPeriodic(); + } + } + + public static class SpinState extends TransferRollerState { + @Override + public void periodic( + StateMachine stateMachine, + TransferRollerSubsystem transferRoller) { + transferRoller.controlToTargetVelocity(); + } + } + + public static class DeJamState extends TransferRollerState { + @Override + public void periodic( + StateMachine stateMachine, + TransferRollerSubsystem transferRoller) { + transferRoller.controlToTargetVelocity(); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/transferroller/TransferRollerSubsystem.java b/src/main/java/frc/robot/subsystems/transferroller/TransferRollerSubsystem.java new file mode 100644 index 00000000..a6204600 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/transferroller/TransferRollerSubsystem.java @@ -0,0 +1,181 @@ +package frc.robot.subsystems.transferroller; + +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import coppercore.controls.state_machine.StateMachine; +import coppercore.wpilib_interface.MonitoredSubsystem; +import coppercore.wpilib_interface.subsystems.motors.MotorIO; +import coppercore.wpilib_interface.subsystems.motors.MotorInputsAutoLogged; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.constants.JsonConstants; +import frc.robot.subsystems.transferroller.TransferRollerState.DeJamState; +import frc.robot.subsystems.transferroller.TransferRollerState.IdleState; +import frc.robot.subsystems.transferroller.TransferRollerState.SpinState; +import frc.robot.subsystems.transferroller.TransferRollerState.TestModeState; +import frc.robot.util.StateMachineDump; +import frc.robot.util.TestModeManager; +import frc.robot.util.TuningModeHelper; +import frc.robot.util.TuningModeHelper.ControlMode; +import frc.robot.util.TuningModeHelper.MotorTuningMode; +import frc.robot.util.TuningModeHelper.TunableMotor; +import frc.robot.util.TuningModeHelper.TunableMotorConfiguration; +import frc.robot.util.math.Lazy; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + +public class TransferRollerSubsystem extends MonitoredSubsystem { + // Motor and inputs + private final MotorIO motor; + private final MotorInputsAutoLogged inputs = new MotorInputsAutoLogged(); + + // Desired roller velocity + private AngularVelocity targetVelocity = RadiansPerSecond.of(0.0); + + // State machine and states + private final StateMachine stateMachine; + + private final TransferRollerState idleState; + private final TransferRollerState testModeState; + private final TransferRollerState spinState; + private final TransferRollerState deJamState; + + // Tuning Helper and Test Mode Manager + TestModeManager testModeManager = + new TestModeManager("TransferRoller", TestMode.class); + + Lazy> tuningModeHelper; + + public TransferRollerSubsystem(MotorIO motor) { + this.motor = motor; + + // TODO: Define state machine transitions, register states + stateMachine = new StateMachine<>(this); + idleState = stateMachine.registerState(new IdleState()); + testModeState = stateMachine.registerState(new TestModeState()); + spinState = stateMachine.registerState(new SpinState()); + deJamState = stateMachine.registerState(new DeJamState()); + + idleState + .when( + transferRoller -> transferRoller.isTransferRollerTestMode(), + "In transfer roller test mode") + .transitionTo(testModeState); + + idleState + .when(transferRoller -> transferRoller.shouldSpin(), "Postive target velocity requested") + .transitionTo(spinState); + + idleState + .when(transferRoller -> transferRoller.shouldDeJam(), "Negative target velocity requested") + .transitionTo(deJamState); + + testModeState + .when( + transferRoller -> !transferRoller.isTransferRollerTestMode(), + "Not in transfer roller test mode") + .transitionTo(idleState); + + spinState + .when( + transferRoller -> !transferRoller.shouldSpin(), + "Non-positive target velocity requested") + .transitionTo(idleState); + + deJamState + .when( + transferRoller -> !transferRoller.shouldDeJam(), + "Non-negative target velocity requested") + .transitionTo(idleState); + + stateMachine.setState(idleState); + StateMachineDump.write("transferroller", stateMachine); + + // Initialize tuning mode helper + Supplier createTunableMotor = + () -> + TunableMotorConfiguration.defaultConfiguration() + .withVelocityTuning() + .profiled() + .withDefaultMotionProfileConfig( + JsonConstants.transferRollerConstants.transferRollerMotionProfileConfig) + .withDefaultPIDGains(JsonConstants.transferRollerConstants.transferRollerGains) + .onPIDGainsChanged( + newGains -> + JsonConstants.transferRollerConstants.transferRollerGains = newGains) + .onMotionProfileConfigChanged( + newProfile -> + JsonConstants.transferRollerConstants.transferRollerMotionProfileConfig = + newProfile) + .withTunableAngularVelocityUnit(RPM) + .build("TransferRoller/MotorTuning", motor); + + tuningModeHelper = + new Lazy<>( + () -> + new TuningModeHelper(TestMode.class) + .addMotorTuningModes( + createTunableMotor.get(), + MotorTuningMode.of( + TestMode.TransferRollerClosedLoopTuning, ControlMode.CLOSED_LOOP), + MotorTuningMode.of( + TestMode.TransferRollerCurrentTuning, ControlMode.OPEN_LOOP_CURRENT), + MotorTuningMode.of( + TestMode.TransferRollerVoltageTuning, ControlMode.OPEN_LOOP_VOLTAGE), + MotorTuningMode.of( + TestMode.TransferRollerPhoenixTuning, ControlMode.PHOENIX_TUNING), + MotorTuningMode.of(TestMode.None, ControlMode.NONE))); + } + + @Override + public void monitoredPeriodic() { + motor.updateInputs(inputs); + Logger.processInputs("TransferRoller/inputs", inputs); + + Logger.recordOutput("TransferRoller/State", stateMachine.getCurrentState().getName()); + stateMachine.periodic(); + Logger.recordOutput("TransferRoller/StateAfter", stateMachine.getCurrentState().getName()); + } + + protected void testPeriodic() { + tuningModeHelper.get().testPeriodic(testModeManager.getTestMode()); + } + + /** + * Check TestModeManager for whether or not the currently selected test mode requires the transfer + * roller to switch to its tuning state. + * + * @return True if the robot is enabled in test mode with a transfer roller test mode selected, + * false if a non-transfer roller test mode is selected, the test mode doesn't require the + * transfer roller to enter TestModeState, the robot isn't enabled in test mode, or + * TestModeManager hasn't been initialized. + */ + private boolean isTransferRollerTestMode() { + return testModeManager.isInTestMode(); + } + + public AngularVelocity getVelocity() { + return RadiansPerSecond.of(inputs.velocityRadiansPerSecond); + } + + public void controlToTargetVelocity() { + motor.controlToVelocityProfiled(targetVelocity); + } + + public void setTargetVelocity(AngularVelocity velocity) { + targetVelocity = velocity; + } + + public boolean shouldSpin() { + return targetVelocity.in(Units.RadiansPerSecond) > 0; + } + + public boolean shouldDeJam() { + return targetVelocity.in(Units.RadiansPerSecond) < 0; + } + + void coast() { + motor.controlCoast(); + } +} diff --git a/state_machine_graphs/transferRoller.dot b/state_machine_graphs/transferRoller.dot new file mode 100644 index 00000000..dc6ea079 --- /dev/null +++ b/state_machine_graphs/transferRoller.dot @@ -0,0 +1,34 @@ +digraph { + + // Graphviz Format settings + + node [ + shape=box, + style=rounded + ]; + + edge [ + fontsize=10 + ]; + + + // States + + SpinState ; + DeJamState ; + IdleState ; + TestModeState ; + + // Transitions + + SpinState -> IdleState [label="Non-positive target velocity requested"]; + + DeJamState -> IdleState [label="Non-negative target velocity requested"]; + + IdleState -> TestModeState [label="In transfer roller test mode"]; + IdleState -> SpinState [label="Postive target velocity requested"]; + IdleState -> DeJamState [label="Negative target velocity requested"]; + + TestModeState -> IdleState [label="Not in transfer roller test mode"]; + +}