From 87d5c1322f83d6515f050a3d174cd831b5fcb86a Mon Sep 17 00:00:00 2001
From: Aarav <91212@psdschools.org>
Date: Thu, 9 Jan 2020 18:05:16 -0700
Subject: [PATCH] Added Pigeon APIs

Ability to get Yaw, Pitch, and Roll, and reset the Yaw.
---
 src/main/java/frc4388/robot/Constants.java    |  3 ++-
 .../java/frc4388/robot/subsystems/Drive.java  | 26 +++++++++++++++++++
 2 files changed, 28 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 933908d..a0385ee 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -22,7 +22,8 @@ public final class Constants {
         public static final int DRIVE_LEFT_FRONT_CAN_ID = 2;
         public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4;
 	    public static final int DRIVE_LEFT_BACK_CAN_ID = 3;
-	    public static final int DRIVE_RIGHT_BACK_CAN_ID = 5;
+        public static final int DRIVE_RIGHT_BACK_CAN_ID = 5;
+        public static final int PIGEON_ID = 6;
     }
 
     public static final class LEDConstants {
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index af19272..4a369d9 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -10,6 +10,7 @@ package frc4388.robot.subsystems;
 import com.ctre.phoenix.motorcontrol.InvertType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.ctre.phoenix.sensors.PigeonIMU;
 
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -27,6 +28,7 @@ public class Drive extends SubsystemBase {
   public static WPI_TalonSRX m_rightFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
   public static WPI_TalonSRX m_leftBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
   public static WPI_TalonSRX m_rightBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
+  public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
 
   public static DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
 
@@ -39,6 +41,7 @@ public class Drive extends SubsystemBase {
     m_rightFrontMotor.configFactoryDefault();
     m_leftBackMotor.configFactoryDefault();
     m_rightBackMotor.configFactoryDefault();
+    m_pigeon.configFactoryDefault();
 
     /* set back motors as followers */
     m_leftBackMotor.follow(m_leftFrontMotor);
@@ -63,4 +66,27 @@ public class Drive extends SubsystemBase {
   public void driveWithInput(double move, double steer){
     m_driveTrain.arcadeDrive(move, steer);
   }
+
+  public double getGyroYaw() {
+    double[] ypr = new double[3];
+    m_pigeon.getYawPitchRoll(ypr);
+    return ypr[0];
+  }
+
+  public double getGyroPitch() {
+    double[] ypr = new double[3];
+    m_pigeon.getYawPitchRoll(ypr);
+    return ypr[1];
+  }
+  
+  public double getGyroRoll() {
+    double[] ypr = new double[3];
+    m_pigeon.getYawPitchRoll(ypr);
+    return ypr[2];
+  }
+
+  public void resetGyroYaw() {
+    m_pigeon.setYaw(0);
+    m_pigeon.setAccumZAngle(0);
+  }
 }