diff --git a/.vscode/settings.json b/.vscode/settings.json index e14bee1..71d91bf 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -63,7 +63,7 @@ "edu.wpi.first.math.**.struct.*" ], "java.dependency.enableDependencyCheckup": false, - "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle", + "editor.defaultFormatter": "redhat.java", "[json]": { "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" }, diff --git a/advantagescope/Robot_1572026/config.json b/advantagescope/Robot_1572026/config.json deleted file mode 100755 index 984de79..0000000 --- a/advantagescope/Robot_1572026/config.json +++ /dev/null @@ -1,106 +0,0 @@ -{ - "name": "Mantis II", - "isFTC": "false", - "rotations": [ - { - "axis": "x", - "degrees": 90 - }, - { - "axis": "y", - "degrees": 0 - }, - { - "axis": "z", - "degrees": 90 - } - ], - "position": [ - 0, - 0, - 0 - ], - "cameras": [ - { - "name": "Left Camera", - "rotations": [ - { - "axis": "x", - "degrees": 0 - }, - { - "axis": "y", - "degrees": 0 - }, - { - "axis": "z", - "degrees": 25 - } - ], - "position": [ - -0.136, - 0.310, - 0.394 - ], - "resolution": [ - 640, - 480 - ], - "fov": 80 - }, - { - "name": "Right Camera", - "rotations": [ - { - "axis": "x", - "degrees": 0 - }, - { - "axis": "y", - "degrees": 0 - }, - { - "axis": "z", - "degrees": -25 - } - ], - "position": [ - -0.136, - -0.310, - 0.394 - ], - "resolution": [ - 640, - 480 - ], - "fov": 80 - }, - { - "name": "Back Camera", - "rotations": [ - { - "axis": "x", - "degrees": 0 - }, - { - "axis": "y", - "degrees": 0 - }, - { - "axis": "z", - "degrees": 180 - } - ], - "position": [ - -0.301, - 0, - 0.351 - ], - "resolution": [ - 640, - 480 - ], - "fov": 55 - } - ] -} diff --git a/advantagescope/Robot_15720262/config.json b/advantagescope/Robot_15720262/config.json deleted file mode 100755 index 679911c..0000000 --- a/advantagescope/Robot_15720262/config.json +++ /dev/null @@ -1,106 +0,0 @@ -{ - "name": "Mantis II Deployed", - "isFTC": "false", - "rotations": [ - { - "axis": "x", - "degrees": 90 - }, - { - "axis": "y", - "degrees": 0 - }, - { - "axis": "z", - "degrees": 90 - } - ], - "position": [ - 0, - 0, - 0 - ], - "cameras": [ - { - "name": "Left Camera", - "rotations": [ - { - "axis": "x", - "degrees": 0 - }, - { - "axis": "y", - "degrees": 0 - }, - { - "axis": "z", - "degrees": 25 - } - ], - "position": [ - -0.136, - 0.310, - 0.394 - ], - "resolution": [ - 640, - 480 - ], - "fov": 80 - }, - { - "name": "Right Camera", - "rotations": [ - { - "axis": "x", - "degrees": 0 - }, - { - "axis": "y", - "degrees": 0 - }, - { - "axis": "z", - "degrees": -25 - } - ], - "position": [ - -0.136, - -0.310, - 0.394 - ], - "resolution": [ - 640, - 480 - ], - "fov": 80 - }, - { - "name": "Back Camera", - "rotations": [ - { - "axis": "x", - "degrees": 0 - }, - { - "axis": "y", - "degrees": 0 - }, - { - "axis": "z", - "degrees": 180 - } - ], - "position": [ - -0.301, - 0, - 0.351 - ], - "resolution": [ - 640, - 480 - ], - "fov": 55 - } - ] -} diff --git a/advantagescope/Robot_15720262/model.glb b/advantagescope/Robot_15720262/model.glb deleted file mode 100644 index 662c7cc..0000000 Binary files a/advantagescope/Robot_15720262/model.glb and /dev/null differ diff --git a/advantagescope/Robot_Mantiis/model.glb b/advantagescope/Robot_Mantiis/model.glb deleted file mode 100644 index a6eaae8..0000000 Binary files a/advantagescope/Robot_Mantiis/model.glb and /dev/null differ diff --git a/advantagescope/Robot_Mantiis/model_0.glb b/advantagescope/Robot_Mantiis/model_0.glb deleted file mode 100644 index f04d95b..0000000 Binary files a/advantagescope/Robot_Mantiis/model_0.glb and /dev/null differ diff --git a/advantagescope/Robot_Mantiis/model_1.glb b/advantagescope/Robot_Mantiis/model_1.glb deleted file mode 100644 index c4a2483..0000000 Binary files a/advantagescope/Robot_Mantiis/model_1.glb and /dev/null differ diff --git a/advantagescope/Robot_Mantiis/model_2.glb b/advantagescope/Robot_Mantiis/model_2.glb deleted file mode 100644 index 1c246c5..0000000 Binary files a/advantagescope/Robot_Mantiis/model_2.glb and /dev/null differ diff --git a/advantagescope/Robot_Mantiis/model_3.glb b/advantagescope/Robot_Mantiis/model_3.glb deleted file mode 100644 index 123a452..0000000 Binary files a/advantagescope/Robot_Mantiis/model_3.glb and /dev/null differ diff --git a/advantagescope/Robot_Mantiis/config.json b/advantagescope/Robot_Sunstone/config.json similarity index 87% rename from advantagescope/Robot_Mantiis/config.json rename to advantagescope/Robot_Sunstone/config.json index 98bcb24..0e23639 100755 --- a/advantagescope/Robot_Mantiis/config.json +++ b/advantagescope/Robot_Sunstone/config.json @@ -1,5 +1,5 @@ { - "name": "Mantis II Articulated", + "name": "Sunstone", "isFTC": "false", "rotations": [ { @@ -38,9 +38,9 @@ } ], "position": [ - -0.136, - 0.310, - 0.394 + -0.127, + 0.3392, + 0.3556 ], "resolution": [ 640, @@ -65,9 +65,9 @@ } ], "position": [ - -0.136, - -0.310, - 0.394 + -0.127, + -0.3302, + 0.3556 ], "resolution": [ 640, @@ -92,19 +92,20 @@ } ], "position": [ - -0.301, + -0.32326, 0, - 0.351 + 0.3556 ], "resolution": [ 640, 480 ], - "fov": 55 + "fov": 92 } ], "components": [ { + "name": "Turret Base", "zeroedRotations": [ { "axis": "x", @@ -120,12 +121,13 @@ } ], "zeroedPosition": [ - 0.171, - 0, - -0.460 + 0.12192, + 0.11557, + -0.396535 ] }, { + "name": "Hood", "zeroedRotations": [ { "axis": "x", @@ -141,12 +143,13 @@ } ], "zeroedPosition": [ - 0.0465, - 0, - -0.530 + 0.0223, + 0.115, + -0.46 ] }, { + "name": "Slapdown", "zeroedRotations": [ { "axis": "x", @@ -168,6 +171,7 @@ ] }, { + "name": "Hopper Walls", "zeroedRotations": [ { "axis": "x", @@ -183,7 +187,7 @@ } ], "zeroedPosition": [ - -0.3048, + 0, 0, 0 ] diff --git a/advantagescope/Robot_1572026/model.glb b/advantagescope/Robot_Sunstone/model.glb similarity index 82% rename from advantagescope/Robot_1572026/model.glb rename to advantagescope/Robot_Sunstone/model.glb index 6127c0e..e0d7442 100644 Binary files a/advantagescope/Robot_1572026/model.glb and b/advantagescope/Robot_Sunstone/model.glb differ diff --git a/advantagescope/Robot_Sunstone/model_0.glb b/advantagescope/Robot_Sunstone/model_0.glb new file mode 100644 index 0000000..8d8269b Binary files /dev/null and b/advantagescope/Robot_Sunstone/model_0.glb differ diff --git a/advantagescope/Robot_Sunstone/model_1.glb b/advantagescope/Robot_Sunstone/model_1.glb new file mode 100644 index 0000000..a2af34e Binary files /dev/null and b/advantagescope/Robot_Sunstone/model_1.glb differ diff --git a/advantagescope/Robot_Sunstone/model_2.glb b/advantagescope/Robot_Sunstone/model_2.glb new file mode 100644 index 0000000..80f17b0 Binary files /dev/null and b/advantagescope/Robot_Sunstone/model_2.glb differ diff --git a/advantagescope/Robot_Sunstone/model_3.glb b/advantagescope/Robot_Sunstone/model_3.glb new file mode 100644 index 0000000..3fce683 Binary files /dev/null and b/advantagescope/Robot_Sunstone/model_3.glb differ diff --git a/build.gradle b/build.gradle index 26b8860..797e53b 100644 --- a/build.gradle +++ b/build.gradle @@ -212,7 +212,7 @@ spotless { exclude "**/build/**", "**/build-*/**" } trimTrailingWhitespace() - indentWithSpaces(2) + indentWithSpaces(4) endWithNewline() } } diff --git a/plans.txt b/plans.txt deleted file mode 100644 index 5b654d6..0000000 --- a/plans.txt +++ /dev/null @@ -1,59 +0,0 @@ -shooter (start stop change speed) ( shot 15 shots with 80 percent accuracy) - - azmimuth - -(change angle of shot) - - limit switch both sides - - pos sensor - - liekly 5 or 10 turn pot - -(return to default pos) - -? how figure out where default positon is - -angle of motion = 32.5 deg - - - - turret - -(change heading of shot) - - limit switch both sides - - pos sensor - -(return to default pos) - -? how figure out where default positon is - - flywheels - - (spin up) - - (match calculated trajectory) - - ? max rpm - -? how many motors, what motors do, limits, sensor?, servos? - -? intake size/intake rate - -intake - - over the bumper - - (deploy) - - (recall) - - (start) - - (stop) - -? how many motors, what motors do, limits, sensor?, servos? - -? size, intake capability, - -drive - - swerve pods - - wheel distance 22 in - - radius 2in - - steer ratio = 24:1 - - drive ratio = 6.23:1 - - -uptake - - ???? - - (on) - - (off) - -vision - - multiple tags on one camera - - multiple cameras? - - (ball spotting??????) - - ? how many cameras - -cliber/hangar/uppies - - ? how climb - - (climb l1) - - (enhanced climb aka L2 and L3) - - (climb back down) - - ? how many motors, what motors do, limits, sensor?, servos? - diff --git a/src/main/java/org/team157/robot/BuildConstants.java b/src/main/java/org/team157/robot/BuildConstants.java index 06f523d..76e54c5 100644 --- a/src/main/java/org/team157/robot/BuildConstants.java +++ b/src/main/java/org/team157/robot/BuildConstants.java @@ -5,13 +5,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "FRC2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 351; - public static final String GIT_SHA = "652ff3c790222aa517431751ea1e0f27dcaca6fe"; - public static final String GIT_DATE = "2026-05-12 17:02:02 EDT"; - public static final String GIT_BRANCH = "feature/vision-advantagekit"; - public static final String BUILD_DATE = "2026-05-12 19:21:32 EDT"; - public static final long BUILD_UNIX_TIME = 1778628092532L; - public static final int DIRTY = 1; + public static final int GIT_REVISION = 366; + public static final String GIT_SHA = "2bdd41328abdfc8999eab7d5cad6e80f4c55241c"; + public static final String GIT_DATE = "2026-05-15 11:56:30 EDT"; + public static final String GIT_BRANCH = "task/reexport-as-model"; + public static final String BUILD_DATE = "2026-05-15 15:57:29 EDT"; + public static final long BUILD_UNIX_TIME = 1778875049705L; + public static final int DIRTY = 0; private BuildConstants() {} } diff --git a/src/main/java/org/team157/robot/Constants.java b/src/main/java/org/team157/robot/Constants.java index cecb3b9..e92e4b9 100644 --- a/src/main/java/org/team157/robot/Constants.java +++ b/src/main/java/org/team157/robot/Constants.java @@ -12,8 +12,6 @@ import com.ctre.phoenix6.CANBus; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.RobotBase; import org.team157.robot.parsing.PositionDetails; @@ -86,19 +84,4 @@ public static class FieldConstants { public static final PositionDetails positionDetails = new PositionDetails(); } - - public static class ModelConstants { - // 3D offsets from the robot's origin (center of rotation) to various key points - // on the robot, used for mechanism visualization on the AdvantageScope model. - public static final Translation3d ORIGIN_TO_TURRET_BASE_OFFSET = - new Translation3d(-0.12192, -0.11557, 0.396535); - public static final Translation3d ORIGIN_TO_HOOD_PIVOT_POINT_OFFSET = - new Translation3d(-0.0465, 0, 0.530); - public static final Translation3d ORIGIN_TO_INTAKE_PIVOT_POINT_OFFSET = - new Translation3d(0.146050, 0, 0.197803); - // 2D offset from the robot's origin to the turret base, used in position-based - // dynamic shooting calculations. - public static final Transform2d XY_ORIGIN_TO_TURRET_BASE_OFFSET = - new Transform2d(-0.12192, -0.11557, new Rotation2d()); - } } diff --git a/src/main/java/org/team157/robot/Robot.java b/src/main/java/org/team157/robot/Robot.java index 8941ee0..2eaaddf 100644 --- a/src/main/java/org/team157/robot/Robot.java +++ b/src/main/java/org/team157/robot/Robot.java @@ -4,10 +4,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; @@ -111,27 +108,6 @@ public void robotPeriodic() { // Return to non-RT thread priority (do not modify the first argument) // Threads.setCurrentThreadPriority(false, 10); - Logger.recordOutput( - "FinalComponentPoses", - new Pose3d[] { - // turret base - m_robotContainer.turret.getBasePose(), - // turret hood - m_robotContainer.turret.getHoodPivotPose( - new Transform3d( - 0, - 0, - 0, - new Rotation3d( - 0, - Math.toRadians( - m_robotContainer.hood.getScaledPosAngleSim()), - 0))), - // intake pivot - m_robotContainer.slapdown.getIntakePivotPose(), - // hopper walls - m_robotContainer.slapdown.getHopperWallsPose() - }); Logger.recordOutput("Misc/Manual Override Active?", RobotContainer.manualOverride); // Gets the match time from the FMS to display for the driver. Logger.recordOutput("Misc/Match Time", Timer.getMatchTime()); diff --git a/src/main/java/org/team157/robot/RobotContainer.java b/src/main/java/org/team157/robot/RobotContainer.java index b1b8325..6b855fe 100644 --- a/src/main/java/org/team157/robot/RobotContainer.java +++ b/src/main/java/org/team157/robot/RobotContainer.java @@ -25,6 +25,7 @@ import org.team157.robot.commands.DriveCommands; import org.team157.robot.generated.TunerConstants; import org.team157.robot.subsystems.LEDs; +import org.team157.robot.subsystems.SunstoneMechanism3D; import org.team157.robot.subsystems.drive.Drive; import org.team157.robot.subsystems.drive.GyroIO; import org.team157.robot.subsystems.drive.GyroIOPigeon2; @@ -76,14 +77,15 @@ public class RobotContainer { // Subsystems public static Vision vision; public static Drive drive; - public final Turret turret = new Turret(); + public static SunstoneMechanism3D mechanism3D; + public static final Turret turret = new Turret(); public static final Flywheel flywheel = new Flywheel(); - public final Hood hood = new Hood(); - public final Intake intake = new Intake(); - public final Hopper hopper = new Hopper(); - public final Uptake uptake = new Uptake(); - public final Slapdown slapdown = new Slapdown(); - public final LEDs leds = new LEDs(); + public static final Hood hood = new Hood(); + public static final Intake intake = new Intake(); + public static final Hopper hopper = new Hopper(); + public static final Uptake uptake = new Uptake(); + public static final Slapdown slapdown = new Slapdown(); + public static final LEDs leds = new LEDs(); // Controllers private final CommandXboxController driverController = new CommandXboxController(0); @@ -98,6 +100,8 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, IO devices, and commands. */ public RobotContainer() { + mechanism3D = new SunstoneMechanism3D(turret, hood, slapdown); + switch (Constants.currentMode) { case REAL: // Real robot, instantiate hardware IO implementations diff --git a/src/main/java/org/team157/robot/subsystems/SunstoneMechanism3D.java b/src/main/java/org/team157/robot/subsystems/SunstoneMechanism3D.java new file mode 100644 index 0000000..6b9c705 --- /dev/null +++ b/src/main/java/org/team157/robot/subsystems/SunstoneMechanism3D.java @@ -0,0 +1,181 @@ +package org.team157.robot.subsystems; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; +import org.team157.robot.subsystems.hood.Hood; +import org.team157.robot.subsystems.slapdown.Slapdown; +import org.team157.robot.subsystems.slapdown.SlapdownConstants; +import org.team157.robot.subsystems.turret.Turret; +import org.team157.utilities.PosUtils; + +/** + * Contains 3D mechanism offset constants for Sunstone V2 and methods to animate the AdvantageScope + * CAD model. + */ +public class SunstoneMechanism3D extends SubsystemBase { + + private Turret turret; + private Hood hood; + private Slapdown slapdown; + + /** + * Initializes the 3D mechanisms for SunstoneV2, and specifies the mechanisms to extract poses + * from. + */ + public SunstoneMechanism3D(Turret turret, Hood hood, Slapdown slapdown) { + this.turret = turret; + this.hood = hood; + this.slapdown = slapdown; + } + + /** + * 3D offsets from the robot's origin (center of rotation, roughly equivalent to the location of + * the Pigeon) to various key points on the robot, used for ballistic calculations and mechanism + * visualization on the AdvantageScope model. + */ + public class Mechanism3DConstants { + + /** 3D offset from the robot origin to the turret base, in meters. */ + public static final Translation3d ORIGIN_TO_TURRET_BASE_OFFSET = + new Translation3d(-0.12192, -0.11557, 0.396535); + + /** 3D offset from the robot origin to the slapdown's pivot point, in meters. */ + public static final Translation3d ORIGIN_TO_SLAPDOWN_PIVOT_POINT_OFFSET = + new Translation3d(0.146050, 0, 0.197803); + + /** 2D offset from the robot's origin to the turret base, used in ballistic calculations. */ + public static final Transform2d XY_ORIGIN_TO_TURRET_BASE_OFFSET = + new Transform2d(-0.12192, -0.11557, new Rotation2d()); + } + + /** + * Gets the 3D pose of the turret base for mechanism visualization. + * + * @return The {@link Pose3d} of the turret base. + */ + public Pose3d getTurretBasePose() { + return new Pose3d( + Mechanism3DConstants.ORIGIN_TO_TURRET_BASE_OFFSET, + new Rotation3d(0, 0, turret.getTurretRotation().in(Radians))); + } + + /** + * Gets the 3D transform from the turret base to the hood pivot point. + * + * @return The {@link Transform3d} from the turret base to the hood pivot. + */ + public Transform3d getHoodPivotLocation() { + return new Transform3d( + 0.1 * Math.cos(turret.getTurretRotation().in(Radians)), + 0.1 * Math.sin(turret.getTurretRotation().in(Radians)), + 0.070, + new Rotation3d(0, 0, turret.getTurretRotation().in(Radians))); + } + + /** + * Gets the 3D pose of the hood pivot point after applying an additional transform. + * + * @param rotation The additional {@link Transform3d} to apply to the hood pivot location. + * @return The resulting {@link Pose3d} of the hood pivot. + */ + public Pose3d getHoodPivotPose(Transform3d rotation) { + return new Pose3d(Mechanism3DConstants.ORIGIN_TO_TURRET_BASE_OFFSET, new Rotation3d()) + .transformBy(getHoodPivotLocation()) + .transformBy(rotation); + } + + /** + * Gets the current 3D pose of the hood for mechanism visualization. + * + * @return a {@link Pose3d} containing the hood's position relative to the robot origin. + */ + public Pose3d getHoodPose() { + return getHoodPivotPose( + new Transform3d( + 0, + 0, + 0, + new Rotation3d(0, Math.toRadians(hood.getScaledPosAngleSim()), 0))); + } + + /** + * Gets the current position of the hopper walls, based on the current angle of the slapdown. + * + * @return the current displacement of the hopper walls from their stowed position, in meters. + */ + public double getHopperWallsPosition() { + return PosUtils.mapRange( + slapdown.getSlapdownAngle().in(Degrees), + SlapdownConstants.MIN_ANGLE, + SlapdownConstants.MAX_ANGLE, + 0.3048, + 0); + } + + /** + * Gets the current 3D pose of the hopper walls for mechanism visualization. + * + * @return a {@link Pose3d} containing the position of the hopper walls relative to the robot + * origin + */ + public Pose3d getHopperWallsPose() { + return new Pose3d(getHopperWallsPosition(), 0, 0, new Rotation3d()); + } + + /** + * Gets the current 3D pose of the slapdown for mechanism visualization. + * + * @return a {@link Pose3d} containing the position of the hopper walls relative to the robot + * origin + */ + public Pose3d getSlapdownPose() { + return new Pose3d( + Mechanism3DConstants.ORIGIN_TO_SLAPDOWN_PIVOT_POINT_OFFSET, + new Rotation3d(0, -slapdown.getSlapdownAngle().in(Radians), 0)); + } + + /** + * Gets the poses of all mechanisms to log via AdvantageKit for mechanism visualization. + * + * @return a {@link Pose3d} array containing the poses of the turret, hood, slapdown, and hopper + * walls. + */ + public Pose3d[] getMechanismPoses() { + return new Pose3d[] { + getTurretBasePose(), // + getHoodPose(), // + getSlapdownPose(), // + getHopperWallsPose() + }; + } + + /** + * Gets an array of blank poses for model calibration + * + * @return a {@link Pose3d} array containing blank Pose3d objects. + */ + public Pose3d[] getCalibrationPoses() { + return new Pose3d[] { + new Pose3d(), // + new Pose3d(), // + new Pose3d(), // + new Pose3d() + }; + } + + @Override + public void periodic() { + // Publish poses to NT and/or save them to the log file. + Logger.recordOutput("Odometry/Mechanism Poses", getMechanismPoses()); + // Uncomment the line below to calibrate pose offsets + // Logger.recordOutput("Odometry/Calibration Poses", getCalibrationPoses()); + } +} diff --git a/src/main/java/org/team157/robot/subsystems/drive/Drive.java b/src/main/java/org/team157/robot/subsystems/drive/Drive.java index 443d256..fe46322 100644 --- a/src/main/java/org/team157/robot/subsystems/drive/Drive.java +++ b/src/main/java/org/team157/robot/subsystems/drive/Drive.java @@ -47,8 +47,8 @@ import org.team157.robot.Constants; import org.team157.robot.Constants.FieldConstants; import org.team157.robot.Constants.Mode; -import org.team157.robot.Constants.ModelConstants; import org.team157.robot.generated.TunerConstants; +import org.team157.robot.subsystems.SunstoneMechanism3D.Mechanism3DConstants; import org.team157.robot.util.LocalADStarAK; public class Drive extends SubsystemBase { @@ -384,7 +384,7 @@ public static Translation2d[] getModuleTranslations() { */ public boolean isUnderTrench() { return FieldConstants.positionDetails.isUnderTrench( - getPose().plus(ModelConstants.XY_ORIGIN_TO_TURRET_BASE_OFFSET)); + getPose().plus(Mechanism3DConstants.XY_ORIGIN_TO_TURRET_BASE_OFFSET)); } /** diff --git a/src/main/java/org/team157/robot/subsystems/hood/Hood.java b/src/main/java/org/team157/robot/subsystems/hood/Hood.java index 7c37ec3..ce418fa 100644 --- a/src/main/java/org/team157/robot/subsystems/hood/Hood.java +++ b/src/main/java/org/team157/robot/subsystems/hood/Hood.java @@ -4,14 +4,11 @@ package org.team157.robot.subsystems.hood; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; -import org.team157.robot.Constants.ModelConstants; import org.team157.robot.subsystems.drive.Drive; import org.team157.robot.subsystems.flywheel.Flywheel; import org.team157.utilities.PosUtils; @@ -116,12 +113,4 @@ public void simulationPeriodic() { // This method will be called once per scheduler run during simulation io.simIterate(); } - - // TODO: make a Mechanism3d class distinct from each subsystem and RobotContainer for storing - // poses for the model - public Pose3d getHoodPose() { - return new Pose3d( - ModelConstants.ORIGIN_TO_HOOD_PIVOT_POINT_OFFSET, - new Rotation3d(0, -(inputs.angleDegrees), 0)); - } } diff --git a/src/main/java/org/team157/robot/subsystems/slapdown/Slapdown.java b/src/main/java/org/team157/robot/subsystems/slapdown/Slapdown.java index 7d53601..f50383d 100644 --- a/src/main/java/org/team157/robot/subsystems/slapdown/Slapdown.java +++ b/src/main/java/org/team157/robot/subsystems/slapdown/Slapdown.java @@ -1,14 +1,11 @@ package org.team157.robot.subsystems.slapdown; -import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.*; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; -import org.team157.robot.Constants.ModelConstants; import org.team157.utilities.PosUtils; /** @@ -120,22 +117,8 @@ public void simulationPeriodic() { io.simIterate(); } - public double getHopperWallsPosition() { - return PosUtils.mapRange( - inputs.angleFromEncoderDegrees, - SlapdownConstants.MIN_ANGLE, - SlapdownConstants.MAX_ANGLE, - 0.3048, - 0); - } - - public Pose3d getHopperWallsPose() { - return new Pose3d(getHopperWallsPosition(), 0, 0, new Rotation3d()); - } - - public Pose3d getIntakePivotPose() { - return new Pose3d( - ModelConstants.ORIGIN_TO_INTAKE_PIVOT_POINT_OFFSET, - new Rotation3d(0, -Math.toRadians(inputs.angleDegrees), 0)); + /** Gets the angle of the pivot to pose the slapdown and hopper walls in the Mechanism3D */ + public Angle getSlapdownAngle() { + return Radians.of(inputs.angleFromEncoderDegrees); } } diff --git a/src/main/java/org/team157/robot/subsystems/turret/Turret.java b/src/main/java/org/team157/robot/subsystems/turret/Turret.java index 5ffc106..83a4d43 100644 --- a/src/main/java/org/team157/robot/subsystems/turret/Turret.java +++ b/src/main/java/org/team157/robot/subsystems/turret/Turret.java @@ -1,16 +1,13 @@ package org.team157.robot.subsystems.turret; -import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.*; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; -import org.team157.robot.Constants.ModelConstants; import org.team157.robot.Robot; import org.team157.robot.subsystems.vision.Vision; @@ -119,39 +116,13 @@ public void updateRelativeAngleToTarget(Pose2d targetPose, Pose2d robotPose) { } /** - * Gets the 3D pose of the turret base for mechanism visualization. + * Gets the 2D rotational pose (yaw) of the turret for mechanism visualization. Feeds into the + * {@link SunstoneMechanism3D} class for the AdvantageScope model. * - * @return The {@link Pose3d} of the turret base. + * @return The {@link Rotation2d} of the turret base. */ - public Pose3d getBasePose() { - return new Pose3d( - ModelConstants.ORIGIN_TO_TURRET_BASE_OFFSET, - new Rotation3d(0, 0, Math.toRadians(inputs.angleDegrees))); - } - - /** - * Gets the 3D transform from the turret base to the hood pivot point. - * - * @return The {@link Transform3d} from the turret base to the hood pivot. - */ - public Transform3d getHoodPivotLocation() { - return new Transform3d( - 0.1245 * Math.cos(Math.toRadians(inputs.angleDegrees)), - 0.1245 * Math.sin(Math.toRadians(inputs.angleDegrees)), - 0.070, - new Rotation3d(0, 0, Math.toRadians(inputs.angleDegrees))); - } - - /** - * Gets the 3D pose of the hood pivot point after applying an additional transform. - * - * @param rotation The additional {@link Transform3d} to apply to the hood pivot location. - * @return The resulting {@link Pose3d} of the hood pivot. - */ - public Pose3d getHoodPivotPose(Transform3d rotation) { - return new Pose3d(ModelConstants.ORIGIN_TO_TURRET_BASE_OFFSET, new Rotation3d()) - .transformBy(getHoodPivotLocation()) - .transformBy(rotation); + public Angle getTurretRotation() { + return Radians.of(Math.toRadians(inputs.angleDegrees)); } @Override diff --git a/src/main/java/org/team157/robot/subsystems/vision/Vision.java b/src/main/java/org/team157/robot/subsystems/vision/Vision.java index 7c55a24..1c8e54a 100644 --- a/src/main/java/org/team157/robot/subsystems/vision/Vision.java +++ b/src/main/java/org/team157/robot/subsystems/vision/Vision.java @@ -28,7 +28,7 @@ import org.littletonrobotics.junction.Logger; import org.photonvision.PhotonUtils; import org.team157.robot.Constants.FieldConstants; -import org.team157.robot.Constants.ModelConstants; +import org.team157.robot.subsystems.SunstoneMechanism3D.Mechanism3DConstants; import org.team157.robot.subsystems.drive.Drive; import org.team157.robot.subsystems.flywheel.Flywheel; import org.team157.robot.subsystems.turret.Turret; @@ -127,15 +127,15 @@ public void setTargetParams(Pose2d targetPose, Pose2d robotPose) { // beginning vector math for momentum shooting double turretToRobotTheta = Math.atan( - ModelConstants.XY_ORIGIN_TO_TURRET_BASE_OFFSET.getY() - / ModelConstants.XY_ORIGIN_TO_TURRET_BASE_OFFSET.getX()); + Mechanism3DConstants.XY_ORIGIN_TO_TURRET_BASE_OFFSET.getY() + / Mechanism3DConstants.XY_ORIGIN_TO_TURRET_BASE_OFFSET.getX()); SimpleMatrix turretToRobotThetaMatrix = new SimpleMatrix( 2, 1, true, -Math.sin(turretToRobotTheta), Math.cos(turretToRobotTheta)); double dOffsetRobot = Math.hypot( - ModelConstants.XY_ORIGIN_TO_TURRET_BASE_OFFSET.getX(), - ModelConstants.XY_ORIGIN_TO_TURRET_BASE_OFFSET.getY()); + Mechanism3DConstants.XY_ORIGIN_TO_TURRET_BASE_OFFSET.getX(), + Mechanism3DConstants.XY_ORIGIN_TO_TURRET_BASE_OFFSET.getY()); SimpleMatrix vRotationRobot = turretToRobotThetaMatrix.scale(driveRotationalVelocity * dOffsetRobot); @@ -183,13 +183,14 @@ public void setTargetParams(Pose2d targetPose, Pose2d robotPose) { distanceToTarget = PhotonUtils.getDistanceToPose(robotPose, adjustedTargetPose); distanceToTargetFromTurret = PhotonUtils.getDistanceToPose( - robotPose.plus(ModelConstants.XY_ORIGIN_TO_TURRET_BASE_OFFSET), + robotPose.plus(Mechanism3DConstants.XY_ORIGIN_TO_TURRET_BASE_OFFSET), adjustedTargetPose); angleToTarget = PhotonUtils.getYawToPose(robotPose, adjustedTargetPose).getDegrees(); angleToTargetFromTurret = PhotonUtils.getYawToPose( - robotPose.plus(ModelConstants.XY_ORIGIN_TO_TURRET_BASE_OFFSET), + robotPose.plus( + Mechanism3DConstants.XY_ORIGIN_TO_TURRET_BASE_OFFSET), adjustedTargetPose) .getDegrees();