From af62961d286f99968acb8a81ae5c7360087e7db7 Mon Sep 17 00:00:00 2001
From: cherriae <zeruiz@nycstudents.net>
Date: Sat, 20 Jan 2024 11:24:21 -0500
Subject: [PATCH] robot speeds

---
 .../java/frc/robot/subsystems/SwerveDriveSubsystem.java  | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

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));
-  }
+      }
+
+  
 
 }