From ae770e71c05ea4b30cc73c68fd630445ad739d04 Mon Sep 17 00:00:00 2001
From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com>
Date: Thu, 12 Jan 2023 18:51:28 -0700
Subject: [PATCH] Commit moment

---
 src/main/java/frc4388/robot/Robot.java       | 11 ++++++++++-
 src/main/java/frc4388/utility/RobotGyro.java | 15 +++++++++------
 2 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index e145b38..2eee91d 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -7,9 +7,13 @@
 
 package frc4388.robot;
 
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
+
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc4388.utility.RobotGyro;
 import frc4388.utility.RobotTime;
 
 /**
@@ -25,6 +29,9 @@ public class Robot extends TimedRobot {
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
 
+  private WPI_Pigeon2 pigeon = new WPI_Pigeon2(14);
+  private RobotGyro gyro = new RobotGyro(pigeon);
+
   /**
    * This function is run when the robot is first started up and should be
    * used for any initialization code.
@@ -113,7 +120,9 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void teleopPeriodic() {
-    
+    SmartDashboard.putNumber("Gyro Yaw", gyro.getYaw());
+    SmartDashboard.putNumber("Gyro Pitch", gyro.getPitch());
+    SmartDashboard.putNumber("Gyro Roll", gyro.getRoll());
   }
 
   /**
diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java
index d3761c6..669122b 100644
--- a/src/main/java/frc4388/utility/RobotGyro.java
+++ b/src/main/java/frc4388/utility/RobotGyro.java
@@ -7,8 +7,7 @@
 
 package frc4388.utility;
 
-import com.ctre.phoenix.sensors.PigeonIMU;
-import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
 import com.kauailabs.navx.frc.AHRS;
 
 // import edu.wpi.first.wpilibj.GyroBase;
@@ -21,7 +20,7 @@ import edu.wpi.first.math.MathUtil;
 public class RobotGyro implements Gyro {
     private RobotTime m_robotTime = RobotTime.getInstance();
 
-    private PigeonIMU m_pigeon = null;
+    private WPI_Pigeon2 m_pigeon = null;
     private AHRS m_navX = null;
     public boolean m_isGyroAPigeon; //true if pigeon, false if navX
 
@@ -36,7 +35,7 @@ public class RobotGyro implements Gyro {
      * Creates a Gyro based on a pigeon
      * @param gyro the gyroscope to use for Gyro
      */
-    public RobotGyro(PigeonIMU gyro) {
+    public RobotGyro(WPI_Pigeon2 gyro) {
         m_pigeon = gyro;
         m_isGyroAPigeon = true;
     }
@@ -90,7 +89,7 @@ public class RobotGyro implements Gyro {
     @Override
     public void calibrate() {
         if (m_isGyroAPigeon) {
-            m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
+            m_pigeon.calibrate();
         } else {
             m_navX.calibrate();
         }
@@ -129,6 +128,10 @@ public class RobotGyro implements Gyro {
         }
     }
 
+    public double getYaw() {
+       return this.getAngle();
+    }
+
     /**
      * Gets an absolute heading of the robot
      * @return heading from -180 to 180 degrees
@@ -182,7 +185,7 @@ public class RobotGyro implements Gyro {
         }
     }
 
-    public PigeonIMU getPigeon(){
+    public WPI_Pigeon2 getPigeon(){
         return m_pigeon;
     }