From 5c7895d2a4da6d845d6342d75661fc5525f3a99f Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 3 Feb 2020 17:31:37 -0700 Subject: [PATCH] Add Motion Magic Aux PID --- src/main/java/frc4388/robot/subsystems/Drive.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 5381f38..0258286 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -340,12 +340,11 @@ public class Drive extends SubsystemBase { m_driveTrain.feedWatchdog(); } - public void runMotionMagicPID(double targetPos){ + public void runMotionMagicPID(double targetPos, double targetGyro){ m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.set(TalonFXControlMode.MotionMagic, targetPos); + m_rightFrontMotor.set(TalonFXControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); - m_driveTrain.feedWatchdog(); }