diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 70217461..8a66d459 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -45,6 +45,7 @@ public static class SwerveConfig { public static final int BACK_LEFT = IS_COMPETITION_ROBOT ? 7 : 0; public static final int BACK_RIGHT = IS_COMPETITION_ROBOT ? 1 : 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; @@ -58,6 +59,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 diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java index e4126ac9..819c2425 100644 --- a/src/main/java/org/team1540/robot2024/Robot.java +++ b/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 @@ -89,6 +90,9 @@ public void robotPeriodic() { // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); + + // Update mechanism visualiser in sim + if (Robot.isSimulation()) MechanismVisualiser.periodic(); } /** diff --git a/src/main/java/org/team1540/robot2024/util/MechanismVisualiser.java b/src/main/java/org/team1540/robot2024/util/MechanismVisualiser.java new file mode 100644 index 00000000..304585e1 --- /dev/null +++ b/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)); + } +}