Intake ctre sim

src/main/java/frc/robot/Constants.java

@@ -11,12 +11,16 @@
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.AngularAccelerationUnit;
+import edu.wpi.first.units.AngularVelocityUnit;
+import edu.wpi.first.units.VoltageUnit;
 import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.Frequency;
 import edu.wpi.first.units.measure.LinearAcceleration;
 import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.units.measure.Per;
 import frc.robot.generated.TunerConstants;
 import frc.robot.utils.VisionPoseEstimator.VisionPoseEstimatorConstants;
 
@@ -88,6 +92,17 @@ public static class SwerveConstants {
   public static class IntakeConstants {
     public static final int feedMotorId = 8;
     public static final int actuatorMotorId = 9;
+
+    public static final Per<VoltageUnit, AngularVelocityUnit> actuatorkV =
+        VoltsPerRadianPerSecond.ofNative(0.1);
+    public static final Per<VoltageUnit, AngularAccelerationUnit> actuatorkA =
+        VoltsPerRadianPerSecondSquared.ofNative(0.1);
+
+    public static final AngularVelocity actuatorVelocity = RadiansPerSecond.of(1);
+    public static final AngularAcceleration actuatorAcceleration = RadiansPerSecondPerSecond.of(2);
+
+    public static final double feedGearRatio = 1;
+    public static final double actuatorGearRatio = 1;
   }
 
   public static class WristevatorConstants {

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

@@ -24,7 +24,7 @@ private Superstructure() {
   /** Passoff from intake (if needed) -> serializer -> manipulator. */
   public static Command passoff(Intake intake, Serializer serializer, Manipulator manipulator) {
     return parallel(
-            intake.set(0, 0).unless(serializer::getBackBeam),
+            intake.set(Math.PI, 0).unless(serializer::getBackBeam),
             serializer.setSpeed(0),
             manipulator.setSpeed(0))
         .withName("Passoff");
@@ -40,7 +40,7 @@ public static Command groundOuttake(Intake intake, Serializer serializer) {
 
   /** Intake from intake -> serializer. */
   public static Command groundIntake(Intake intake, Serializer serializer) {
-    return parallel(intake.set(0, 0).unless(serializer::getBackBeam), serializer.setSpeed(0))
+    return parallel(intake.set(Math.PI, 0).unless(serializer::getBackBeam), serializer.setSpeed(0))
         .until(serializer::getFrontBeam)
         .withName("Ground Intake");
   }

src/main/java/frc/robot/subsystems/Intake.java

@@ -1,16 +1,21 @@
 package frc.robot.subsystems;
 
-import static edu.wpi.first.units.Units.Radians;
-import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.*;
 
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
@@ -23,6 +28,7 @@
 import frc.lib.FaultLogger;
 import frc.robot.Constants;
 import frc.robot.Constants.IntakeConstants;
+import frc.robot.Robot;
 
 public class Intake extends AdvancedSubsystem {
   private final Mechanism2d _mech = new Mechanism2d(1.85, 1);
@@ -41,11 +47,37 @@ public class Intake extends AdvancedSubsystem {
   private final StatusSignal<Angle> _actuatorPositionGetter = _actuatorMotor.getPosition();
   private final StatusSignal<AngularVelocity> _feedVelocityGetter = _feedMotor.getVelocity();
 
+  private DCMotorSim _actuatorMotorSim;
+  private DCMotorSim _feedMotorSim;
+
+  private double _lastSimTime;
+
+  private Notifier _simNotifier;
+
   public Intake() {
     setDefaultCommand(set(0.0, 0.0));
 
+    if (Robot.isSimulation()) {
+      _actuatorMotorSim =
+          new DCMotorSim(
+              LinearSystemId.createDCMotorSystem(12.0 / 3.14, 12.0 / 6), DCMotor.getKrakenX60(1));
+
+      _feedMotorSim =
+          new DCMotorSim(LinearSystemId.createDCMotorSystem(0.1, 0.01), DCMotor.getKrakenX60(1));
+
+      startSimThread();
+    }
+
     var feedMotorConfigs = new TalonFXConfiguration();
     var actuatorMotorConfigs = new TalonFXConfiguration();
+
+    actuatorMotorConfigs.Slot0.kV = IntakeConstants.actuatorkV.in(VoltsPerRadianPerSecond);
+    actuatorMotorConfigs.Slot0.kA = IntakeConstants.actuatorkA.in(VoltsPerRadianPerSecondSquared);
+
+    actuatorMotorConfigs.MotionMagic.MotionMagicCruiseVelocity =
+        IntakeConstants.actuatorVelocity.in(RadiansPerSecond);
+    actuatorMotorConfigs.MotionMagic.MotionMagicAcceleration =
+        IntakeConstants.actuatorAcceleration.in(RadiansPerSecondPerSecond);
 
     CTREUtil.attempt(() -> _feedMotor.getConfigurator().apply(feedMotorConfigs), _feedMotor);
     CTREUtil.attempt(
@@ -55,6 +87,45 @@ public Intake() {
     FaultLogger.register(_actuatorMotor);
   }
 
+  private void startSimThread() {
+    _lastSimTime = Utils.getCurrentTimeSeconds();
+
+    _simNotifier =
+        new Notifier(
+            () -> {
+              final double currentTime = Utils.getCurrentTimeSeconds();
+              final double deltaTime = currentTime - _lastSimTime;
+
+              var actuatorMotorSimState = _actuatorMotor.getSimState();
+              var feedMotorSimState = _feedMotor.getSimState();
+
+              actuatorMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage());
+              feedMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage());
+
+              _actuatorMotorSim.setInputVoltage(
+                  actuatorMotorSimState.getMotorVoltageMeasure().in(Volts));
+              _feedMotorSim.setInputVoltage(feedMotorSimState.getMotorVoltageMeasure().in(Volts));
+
+              _actuatorMotorSim.update(deltaTime);
+              _feedMotorSim.update(deltaTime);
+
+              actuatorMotorSimState.setRawRotorPosition(
+                  _actuatorMotorSim.getAngularPosition().times(IntakeConstants.actuatorGearRatio));
+              feedMotorSimState.setRawRotorPosition(
+                  _feedMotorSim.getAngularPosition().times(IntakeConstants.feedGearRatio));
+
+              actuatorMotorSimState.setRotorVelocity(
+                  _actuatorMotorSim.getAngularVelocity().times(IntakeConstants.actuatorGearRatio));
+              feedMotorSimState.setRotorVelocity(
+                  _feedMotorSim.getAngularVelocity().times(IntakeConstants.feedGearRatio));
+
+              _lastSimTime = currentTime;
+            });
+
+    _simNotifier.setName("Intake Sim Thread");
+    _simNotifier.startPeriodic(1 / Constants.simUpdateFrequency.in(Hertz));
+  }
+
   @Logged(name = "Angle")
   public double getAngle() {
     return _actuatorPositionGetter.refresh().getValue().in(Radians);

src/main/java/frc/robot/subsystems/Wristevator.java

@@ -9,9 +9,12 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import dev.doglog.DogLog;
 import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import edu.wpi.first.wpilibj.simulation.DIOSim;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
@@ -85,12 +88,13 @@ public Distance getHeight() {
 
   private final DigitalInput _homeSwitch = new DigitalInput(WristevatorConstants.homeSwitch);
 
-  private final DIOSim _homeSwitchSim;
+  private DIOSim _homeSwitchSim;
 
   public Wristevator() {
     if (Robot.isSimulation()) {
       _homeSwitchSim = new DIOSim(_homeSwitch);
-    } else {
+    }
+    else {
       _homeSwitchSim = null;
     }