From 6f8e8f8b8f95e8b97ff4f48278457f2fd4c487f1 Mon Sep 17 00:00:00 2001
From: Aarav Shah <91212@psdschools.org>
Date: Tue, 18 Feb 2020 16:56:56 -0700
Subject: [PATCH] Added Variable to get Right Motor Pos and Vel

---
 src/main/java/frc4388/robot/RobotContainer.java   |  2 +-
 src/main/java/frc4388/robot/subsystems/Drive.java | 11 ++++++++++-
 2 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 2a244bb..941afe0 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -169,7 +169,7 @@ public class RobotContainer {
 
     public void configDriveTrainSensors(FeedbackDevice type) {
         m_robotDrive.configMotorSensor(type);
-    }
+    } 
 
     public void resetOdometry() {
         m_robotDrive.resetGyroAngles();
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index c18a1d6..0d23a08 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -49,6 +49,10 @@ public class Drive extends SubsystemBase {
   public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
   public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
 
+  public double m_rightFrontMotorPos;
+
+  public double m_rightFrontMotorVel;
+
   public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
 
   SendableChooser<Gains> m_chooser = new SendableChooser<Gains>();
@@ -265,6 +269,11 @@ public class Drive extends SubsystemBase {
     m_lastTime = System.currentTimeMillis();
     m_lastAngleYaw = m_currentAngleYaw;
     m_currentAngleYaw = getGyroYaw();
+
+    m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition();
+
+    m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
+
     
     try {
       SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
@@ -459,7 +468,7 @@ public class Drive extends SubsystemBase {
 
     m_pigeon.getYawPitchRoll(ypr);
     return ypr[0];
-  }
+  } 
 
   /**
    * Returns the current pitch of the pigeon