From 575fcc115551adf5617c53df05e56faf9de744ad Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Thu, 27 Jan 2022 18:58:22 -0700
Subject: [PATCH] smartdash pigeon tests (temporary)

---
 .../java/frc4388/robot/subsystems/SwerveDrive.java  | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index fafc14c..7c7d32a 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -17,6 +17,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.RobotMap;
 import frc4388.robot.Constants.SwerveDriveConstants;
@@ -140,6 +141,18 @@ public class SwerveDrive extends SubsystemBase {
   public void periodic() {
     //System.err.println(m_gyro.getFusedHeading() +"    aaa");
     updateOdometry();
+    SmartDashboard.putNumber("Pigeon Absolute Compass Heading", m_gyro.getAbsoluteCompassHeading());
+    SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading());
+    SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
+    SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
+    SmartDashboard.putNumber("Pigeon Compass Heading", m_gyro.getCompassHeading());
+
+    // m_gyro.getAbsoluteCompassHeading();
+    // m_gyro.getYaw();
+    // m_gyro.getRotation2d();
+    // m_gyro.getAngle();
+    // m_gyro.getCompassHeading();
+    // m_gyro.getFusedHeading();
     // m_gyro.setFusedHeadingToCompass();
     // m_gyro.setYawToCompass();
     super.periodic();