Reconfigure motor sensor between auto and teleop

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

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());

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
    */