initial concept of wrist sim

src/main/java/frc/robot/RobotContainer.java

@@ -3,6 +3,7 @@
 import java.util.Map;
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
@@ -11,8 +12,13 @@
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.shuffleboard.SimpleWidget;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -59,6 +65,16 @@
  * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
+    public static Mechanism2d mech =
+        new Mechanism2d(Constants.Swerve.trackWidth, Constants.Swerve.wheelBase);
+    public static MechanismRoot2d root =
+        mech.getRoot("climber", Units.inchesToMeters(12), Constants.Swerve.wheelBase / 2);
+    public static MechanismLigament2d m_elevator = root.append(new MechanismLigament2d("elevator",
+        Units.inchesToMeters(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT), 90));
+    public static MechanismLigament2d m_wrist = m_elevator.append(new MechanismLigament2d("wrist",
+        0.5, Constants.ElevatorWristConstants.SetPoints.MIN_ANGLE.getRadians(), 6,
+        new Color8Bit(Color.kPurple)));
+
     /* Shuffleboard */
     public static ShuffleboardTab mainDriverTab = Shuffleboard.getTab("Main Driver");
 
@@ -125,6 +141,7 @@ public class RobotContainer {
     /**
      */
     public RobotContainer(RobotRunType runtimeType) {
+        SmartDashboard.putData("Mech2d", mech);
         numNoteChooser.setDefaultOption("0", 0);
         for (int i = 0; i < 7; i++) {
             numNoteChooser.addOption(String.valueOf(i), i);
@@ -304,5 +321,4 @@ public Command getAutonomousCommand() {
         Command autocommand = autoChooser.getSelected();
         return autocommand;
     }
-
 }

src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWristIOSim.java

@@ -1,5 +1,42 @@
 package frc.robot.subsystems.elevator_wrist;
 
+import org.littletonrobotics.junction.LoggedRobot;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+
 public class ElevatorWristIOSim implements ElevatorWristIO {
 
+    private final DCMotor wrist = DCMotor.getNEO(1);
+    private final Encoder m_encoder = new Encoder(0, 1);
+    // Simulation classes help us simulate what's going on, including gravity.
+    // This arm sim represents an arm that can travel from -75 degrees (rotated down front)
+    // to 255 degrees (rotated down in the back).
+    private final SingleJointedArmSim m_armSim = new SingleJointedArmSim(wrist, 60.0,
+        SingleJointedArmSim.estimateMOI(Units.inchesToMeters(12), 8.0), Units.inchesToMeters(12),
+        Constants.ElevatorWristConstants.SetPoints.MIN_ANGLE.getRadians(),
+        Constants.ElevatorWristConstants.SetPoints.MAX_ANGLE.getRadians(), true, 0.0);
+    private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+
+    public ElevatorWristIOSim() {
+
+    }
+
+    @Override
+    public void updateInputs(ElevatorWristInputs inputs) {
+        m_armSim.update(LoggedRobot.defaultPeriodSecs);
+        m_encoderSim.setDistance(m_armSim.getAngleRads());
+        RobotContainer.m_wrist.setAngle(Units.radiansToDegrees(m_armSim.getAngleRads()));
+        inputs.wristAbsoluteEncRawValue = Units.radiansToRotations(m_encoderSim.getDistance());
+    }
+
+    @Override
+    public void setWristVoltage(double v) {
+        m_armSim.setInputVoltage(v);
+    }
+
 }