From a5bdb28d377e564f6b0aba48170de017c5dc1f96 Mon Sep 17 00:00:00 2001
From: Aarav Shah <91212@psdschools.org>
Date: Thu, 13 Feb 2020 17:34:59 -0700
Subject: [PATCH] Reconfigure motor sensor between auto and teleop

---
 src/main/java/frc4388/robot/Robot.java            |  4 ++++
 src/main/java/frc4388/robot/RobotContainer.java   |  5 +++++
 src/main/java/frc4388/robot/subsystems/Drive.java | 12 ++++++++++--
 3 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index b23b040..ca009f0 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -7,6 +7,7 @@
 
 package frc4388.robot;
 
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -76,6 +77,7 @@ public class Robot extends TimedRobot {
 
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
     m_robotContainer.resetOdometry();
+    m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
 
     /*
      * String autoSelected = SmartDashboard.getString("Auto Selector",
@@ -100,6 +102,8 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopInit() {
     m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
+    m_robotContainer.configDriveTrainSensors(FeedbackDevice.SensorDifference);
+    
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index fbdc8bb..e074bfb 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -9,6 +9,7 @@ package frc4388.robot;
 
 import java.util.List;
 
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
 import edu.wpi.first.wpilibj.Joystick;
@@ -156,6 +157,10 @@ public class RobotContainer {
         m_robotDrive.setDriveTrainNeutralMode(mode);
     }
 
+    public void configDriveTrainSensors(FeedbackDevice type) {
+        m_robotDrive.configMotorSensor(type);
+    }
+
     public void resetOdometry() {
         m_robotDrive.resetGyroAngles();
         m_robotDrive.setOdometry(new Pose2d());
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 73a358d..161b7f1 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -162,8 +162,7 @@ public class Drive extends SubsystemBase {
     m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */
-    m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorDifference, DriveConstants.PID_PRIMARY,
-                                                    DriveConstants.DRIVE_TIMEOUT_MS);
+    configMotorSensor(FeedbackDevice.SensorDifference);
 
     /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
     m_rightFrontMotor.configSelectedFeedbackCoefficient(  1, // Coefficient
@@ -389,6 +388,15 @@ public class Drive extends SubsystemBase {
     runDriveStraightVelocityPID(moveVel, targetGyro);
   }
 
+  /**
+   * Selects the feedback device for the motors.
+   * @param feedbackDevice The feedback device to set it to, usually SensorDifference or
+   */
+  public void configMotorSensor(FeedbackDevice type) {
+    m_rightFrontMotor.configSelectedFeedbackSensor( type, DriveConstants.PID_PRIMARY,
+                                                    DriveConstants.DRIVE_TIMEOUT_MS);
+  }
+
   /**
    * Returns the current yaw of the pigeon
    */