diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index e283d50..3b2ab05 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -50,6 +50,8 @@ public class SwerveDriveSubsystem extends SubsystemBase { private VisionSubsystem _visionSubsystem; + private double _robotSpeed = 0; + // estimated pose private Pose2d _pose = new Pose2d(); @@ -143,6 +145,9 @@ public void periodic() { _field.setRobotPose(_pose); SmartDashboard.putData("FIELD", _field); + + _robotSpeed = Math.sqrt(Math.pow(getRobotRelativeSpeeds().vxMetersPerSecond, 2) + Math.pow(getRobotRelativeSpeeds().vyMetersPerSecond, 2)); + SmartDashboard.putNumber("DRIVE SPEED (m/s)", _robotSpeed); } /** @@ -218,6 +223,8 @@ public Rotation2d getHeading() { */ public Rotation2d getHeadingRaw() { return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getHeading(), 360)); - } + } + + }