Merge branch 'main' of https://github.com/Team334/R2024

Team334 · Jan 20, 2024 · 360b2e0 · 360b2e0
2 parents 4c78610 + 1334bc1
commit 360b2e0
Showing 1 changed file with 8 additions and 1 deletion.
diff --git 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));
-  }
+      }
+
+
 
 }