Add Sim and Visualizer

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

@@ -40,10 +40,12 @@
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.Vision.Vision;
 import frc.robot.subsystems.Vision.VisionIOPhotonVision;
+import frc.robot.subsystems.Vision.VisionIOPhotonVisionSim;
 import frc.robot.subsystems.algae.Algae;
 import frc.robot.subsystems.algae.AlgaeIOTalonFX;
 import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain;
 import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.elevator.ElevatorIOSim;
 import frc.robot.subsystems.elevator.ElevatorIOTalonFX;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.ShooterIOTalonFX;
@@ -79,7 +81,7 @@ public class RobotContainer {
 
     public RobotContainer() {
         s_Shooter = new Shooter(new ShooterIOTalonFX());
-        s_Elevator = new Elevator(new ElevatorIOTalonFX());
+        s_Elevator = RobotBase.isReal() ? new Elevator(new ElevatorIOTalonFX()) : new Elevator(new ElevatorIOSim());
         s_Algae = new Algae(new AlgaeIOTalonFX());
 
         // Auto Named Commands
@@ -146,6 +148,18 @@ else if (HALUtil.getSerialNumber().equals(TunerConstants.RobotV2)) {
                                     Units.degreesToRadians(-25.414),
                                     Units.degreesToRadians(-50)))));
         }
+        else if (RobotBase.isSimulation()){
+                m_vision = new Vision(drivetrain::addVisionMeasurement, new VisionIOPhotonVisionSim("front-right-cam",new Transform3d(new Translation3d(
+                        Units.inchesToMeters(7.16),
+                        Units.inchesToMeters(-10.92),
+                        Units.inchesToMeters(9.39)),
+                        new Rotation3d(
+                                Units.degreesToRadians(0.0),
+                                Units.degreesToRadians(-21.173),
+                                Units.degreesToRadians(-20))),
+                                ()->drivetrain.getState().Pose)
+                                );
+        }
         else { 
             System.out.println("Unknown Robot");
             throw new RuntimeException("Unknown Robot Serial Number");

src/main/java/frc/robot/generated/TunerConstants.java

@@ -35,6 +35,7 @@
 import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.units.measure.MomentOfInertia;
 import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.RobotBase;
 import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain;
 
 // Generated by the Tuner X Swerve Project Generator
@@ -220,14 +221,22 @@ public static CommandSwerveDrivetrain createDrivetrain() {
             kBackLeftEncoderOffset = kBackLeftEncoderOffsetRobot1;
             kBackRightEncoderOffset = kBackRightEncoderOffsetRobot1;
 
-        } else if (HALUtil.getSerialNumber().equals(RobotV2)) {
+        } 
+        else if (HALUtil.getSerialNumber().equals(RobotV2)) {
             System.out.println("Robot 2");
             kBackLeftEncoderOffset = kBackLeftEncoderOffsetRobot2;
             kBackRightEncoderOffset = kBackRightEncoderOffsetRobot2;
             kFrontLeftEncoderOffset = kFrontLeftEncoderOffsetRobot2;
             kFrontRightEncoderOffset = kFrontRightEncoderOffsetRobot2;
 
-        } else {
+        } 
+        else if (RobotBase.isSimulation()){
+            kBackLeftEncoderOffset = kBackLeftEncoderOffsetRobot2;
+            kBackRightEncoderOffset = kBackRightEncoderOffsetRobot2;
+            kFrontLeftEncoderOffset = kFrontLeftEncoderOffsetRobot2;
+            kFrontRightEncoderOffset = kFrontRightEncoderOffsetRobot2;
+        }
+        else {
             System.out.println("Unknown Robot");
             throw new RuntimeException("Unknown Robot Serial Number");
         }

src/main/java/frc/robot/subsystems/elevator/Elevator.java

@@ -100,7 +100,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
         return m_SysIdRoutine.dynamic(direction);
       }
 
-      public Command runSysID(){
+      public Command runFullSysID(){
         return 
         this.runOnce(SignalLogger::start)
         .andThen(this.sysIdQuasistatic(SysIdRoutine.Direction.kForward))

src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java

@@ -0,0 +1,89 @@
+package frc.robot.subsystems.elevator;
+
+import static edu.wpi.first.units.Units.Volts;
+
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.mechanism.LoggedMechanism2d;
+import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d;
+import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+
+public class ElevatorIOSim implements ElevatorIO{
+  private ElevatorSim elevatorSim;
+  private LoggedMechanism2d elevatorMech = new LoggedMechanism2d(2, 2);
+  private LoggedMechanismRoot2d root = elevatorMech.getRoot("elevatorRoot", 1, 0);
+  private LoggedMechanismLigament2d elevatorModel = root.append(new LoggedMechanismLigament2d("Elevator", Units.inchesToMeters(ElevatorConstants.kL1), 90));
+
+  private double volts;
+  private double goalHeight;
+  TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(80, 160);
+
+  ProfiledPIDController profiledPIDController = new ProfiledPIDController(40, 0, 0.1, constraints);
+
+  ElevatorFeedforward elevatorFF =
+      new ElevatorFeedforward(0, 0, 0);
+
+  public ElevatorIOSim() {
+    Logger.recordOutput("Elevator Sim", elevatorMech);
+    elevatorSim =
+        new ElevatorSim(
+            DCMotor.getKrakenX60(2),
+            12,
+            Units.lbsToKilograms(10),
+            Units.inchesToMeters(1.751),
+            Units.inchesToMeters(ElevatorConstants.kL1),
+            Units.inchesToMeters(ElevatorConstants.kL4),
+            true,
+            Units.inchesToMeters(ElevatorConstants.kL1));
+
+    volts = 0;
+    goalHeight = ElevatorConstants.kL1;
+  }
+
+  @Override
+  public void updateInputs(ElevatorIOInputs elevatorInputs) {
+    elevatorModel.setLength(elevatorSim.getPositionMeters());
+    Logger.recordOutput("Elevator Sim", elevatorMech);
+    elevatorSim.update(0.02);
+    elevatorInputs.leftMotorConnected = true;
+    elevatorInputs.rightMotorConnected = true;
+    elevatorInputs.leftMotorVoltage = volts;
+    elevatorInputs.rightMotorVoltage = volts;
+    elevatorInputs.motorPosition = elevatorSim.getPositionMeters();
+    elevatorInputs.goalPosition = goalHeight;
+    elevatorInputs.velocity = elevatorSim.getVelocityMetersPerSecond();
+
+    double calculatedVolts =
+        profiledPIDController.calculate(
+                elevatorSim.getPositionMeters(), Units.inchesToMeters(goalHeight))
+            + elevatorFF.calculate(profiledPIDController.getSetpoint().velocity);
+    setVoltage(calculatedVolts);
+  }
+
+  @Override
+  public void setVoltage(double voltage) {
+    volts = voltage;
+    elevatorSim.setInputVoltage(MathUtil.clamp(volts, -12, 12));
+  }
+
+  @Override
+  public void gotosetpoint(double setpoint, double gearRatio) {
+    double calculatedVolts =
+        profiledPIDController.calculate(
+                elevatorSim.getPositionMeters(), Units.inchesToMeters(setpoint*gearRatio))
+            + elevatorFF.calculate(profiledPIDController.getSetpoint().velocity);
+    setVoltage(calculatedVolts);
+    goalHeight = setpoint*gearRatio;
+  }
+}