Merge branch 'main' into zhangal/phoenixpro

Author: mimizh2418

src/main/java/org/team1540/robot2024/Constants.java

@@ -48,6 +48,7 @@ public static class SwerveConfig {
         // TODO: set this id
         public static final int PIGEON_ID = 0;
     }
+
     public static class Drivetrain {
         public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
         public static final double TURN_GEAR_RATIO = 150.0 / 7.0;
@@ -61,6 +62,10 @@ public static class Drivetrain {
         public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
     }
 
+    public static class Elevator {
+        public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(21.0);
+    }
+
     public static class Shooter {
         public static class Flywheels {
             // TODO: determine ids

src/main/java/org/team1540/robot2024/Robot.java

@@ -9,6 +9,7 @@
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+import org.team1540.robot2024.util.MechanismVisualiser;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -90,6 +91,9 @@ public void robotPeriodic() {
         // the Command-based framework to work.
         CommandScheduler.getInstance().run();
         if (Constants.currentMode == Constants.Mode.REAL) robotContainer.odometrySignalRefresher.periodic();
+
+        // Update mechanism visualiser in sim
+        if (Robot.isSimulation()) MechanismVisualiser.periodic();
     }
 
     /**

src/main/java/org/team1540/robot2024/util/MechanismVisualiser.java

@@ -0,0 +1,23 @@
+package org.team1540.robot2024.util;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import org.littletonrobotics.junction.Logger;
+
+public class MechanismVisualiser {
+    private static Pose3d shooterPivot = new Pose3d();
+    private static Pose3d elevator = new Pose3d();
+
+    public static void periodic() {
+        Logger.recordOutput("Mechanisms", shooterPivot, elevator);
+    }
+
+    public static void setElevatorPosition(double positionMeters) {
+        elevator = new Pose3d(0.0, 0.0, positionMeters, new Rotation3d());
+    }
+
+    public static void setShooterPivotRotation(Rotation2d rotation) {
+        shooterPivot = new Pose3d(0.0, 0.0, 0.0, new Rotation3d(0, rotation.getRadians(), 0));
+    }
+}