setup wristevator sim motor code

Team334 · Jan 22, 2025 · 223e136 · 223e136
1 parent 1841a2f
commit 223e136
Showing 1 changed file with 8 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/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;
@@ -83,15 +86,17 @@ public Distance getHeight() {
   private VelocityVoltage _elevatorVelocitySetter = new VelocityVoltage(0);
   private VelocityVoltage _wristVelocitySetter = new VelocityVoltage(0);
 
+  private DCMotorSim _leftMotorSim;
+
   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 {
-      _homeSwitchSim = null;
+      _leftMotorSim =
+          new DCMotorSim(LinearSystemId.createDCMotorSystem(0, 0), DCMotor.getKrakenX60(1));
     }
 
     var leftMotorConfigs = new TalonFXConfiguration();