From 3ca350a5b7a8269798e8341251c2cb46d1842581 Mon Sep 17 00:00:00 2001 From: Josh <102478723+realjoshuau@users.noreply.github.com> Date: Sat, 7 Sep 2024 18:13:08 -0700 Subject: [PATCH 1/2] expose pose2d with the field2d --- simgui.json | 11 +++ .../subsystems/swerve/SwerveTelemetry.java | 71 +++++++++---------- 2 files changed, 46 insertions(+), 36 deletions(-) diff --git a/simgui.json b/simgui.json index 1876ef1e..4b158217 100644 --- a/simgui.json +++ b/simgui.json @@ -57,6 +57,17 @@ "/SmartDashboard/Sim Field": "Field2d" }, "windows": { + "/Pose": { + "bottom": 1476, + "height": 8.210550308227539, + "left": 150, + "right": 2961, + "top": 79, + "width": 16.541748046875, + "window": { + "visible": true + } + }, "/Shuffleboard/Intake/Intake Motor": { "window": { "visible": true diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java b/src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java index 2948cd2a..edadf37c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java @@ -7,6 +7,8 @@ package frc.robot.subsystems.swerve; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.Utils; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; @@ -54,50 +56,47 @@ public SwerveTelemetry(double maxSpeed) { private double lastTime = Utils.getCurrentTimeSeconds(); /* Mechanisms to represent the swerve module states */ - private final Mechanism2d[] m_moduleMechanisms = - new Mechanism2d[] { - new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), - }; + private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { + new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), + }; /* A direction and length changing ligament for speed representation */ - private final MechanismLigament2d[] m_moduleSpeeds = - new MechanismLigament2d[] { - m_moduleMechanisms[0] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[2] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - }; + private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + }; /* A direction changing and length constant ligament for module direction */ - private final MechanismLigament2d[] m_moduleDirections = - new MechanismLigament2d[] { - m_moduleMechanisms[0] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - }; + private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; /* Accept the swerve drive state and telemeterize it to smartdashboard */ public void telemeterize(SwerveDriveState state) { /* Telemeterize the pose */ Pose2d pose = state.Pose; fieldTypePub.set("Field2d"); - fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); - + fieldPub.set(new double[] { pose.getX(), pose.getY(), pose.getRotation().getDegrees() }); + Logger.recordOutput("SwervePoseCalculated", pose); /* Telemeterize the robot's general speeds */ double currentTime = Utils.getCurrentTimeSeconds(); double diffTime = currentTime - lastTime; From ccf70fd0b97dd8e0c5e8c076d6e09f63d688df69 Mon Sep 17 00:00:00 2001 From: Josh <102478723+realjoshuau@users.noreply.github.com> Date: Sat, 7 Sep 2024 18:14:46 -0700 Subject: [PATCH 2/2] :sparkles: spotless --- .../subsystems/swerve/SwerveTelemetry.java | 70 ++++++++++--------- 1 file changed, 36 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java b/src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java index edadf37c..72395008 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveTelemetry.java @@ -7,8 +7,6 @@ package frc.robot.subsystems.swerve; -import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.Utils; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; @@ -23,6 +21,7 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; import frc.robot.subsystems.swerve.kit.KitSwerveDrivetrain.SwerveDriveState; +import org.littletonrobotics.junction.Logger; public class SwerveTelemetry { private final double MaxSpeed; @@ -56,46 +55,49 @@ public SwerveTelemetry(double maxSpeed) { private double lastTime = Utils.getCurrentTimeSeconds(); /* Mechanisms to represent the swerve module states */ - private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { - new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), - }; + private final Mechanism2d[] m_moduleMechanisms = + new Mechanism2d[] { + new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), + }; /* A direction and length changing ligament for speed representation */ - private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { - m_moduleMechanisms[0] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[2] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3] - .getRoot("RootSpeed", 0.5, 0.5) - .append(new MechanismLigament2d("Speed", 0.5, 0)), - }; + private final MechanismLigament2d[] m_moduleSpeeds = + new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + }; /* A direction changing and length constant ligament for module direction */ - private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { - m_moduleMechanisms[0] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - }; + private final MechanismLigament2d[] m_moduleDirections = + new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; /* Accept the swerve drive state and telemeterize it to smartdashboard */ public void telemeterize(SwerveDriveState state) { /* Telemeterize the pose */ Pose2d pose = state.Pose; fieldTypePub.set("Field2d"); - fieldPub.set(new double[] { pose.getX(), pose.getY(), pose.getRotation().getDegrees() }); + fieldPub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); Logger.recordOutput("SwervePoseCalculated", pose); /* Telemeterize the robot's general speeds */ double currentTime = Utils.getCurrentTimeSeconds();