From 097f7414dba0a8e6ce5fb92f6e9a8da20cdb4414 Mon Sep 17 00:00:00 2001
From: mayabartels <mayabartels01@gmail.com>
Date: Mon, 13 Jan 2020 19:26:25 -0700
Subject: [PATCH] Add Configs for Drive PID

---
 src/main/java/frc4388/robot/Constants.java    | 13 +++----
 .../java/frc4388/robot/subsystems/Drive.java  | 36 +++++++++++++++++--
 2 files changed, 39 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 6ba3ae9..b7a4e4c 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -25,14 +25,11 @@ public final class Constants {
         public static final int DRIVE_RIGHT_BACK_CAN_ID = 5;
         public static final int PIGEON_ID = 6;
 
-        /* PID Constants */
-        public static final int SLOT_IDX = 0;
-
-        public static final int PID_LOOP_IDX = 0;
-
-        public static final int TIMEOUT_MS = 30;
-
-        static final Gains kGains = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0);    
+        /* PID Constants Drive*/
+        public static final int DRIVE_SLOT_IDX = 0;
+        public static final int DRIVE_PID_LOOP_IDX = 0;
+        public static final int DRIVE_TIMEOUT_MS = 30;
+        public static final Gains DRIVE_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0);    
         
         public static final double ENCODER_TICKS_PER_REV = 2048;
     }
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index fe12acd..ab834c7 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -11,6 +11,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.InvertType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.ctre.phoenix.sensors.PigeonIMU;
@@ -67,8 +68,39 @@ public class Drive extends SubsystemBase {
     m_rightBackMotor.setInverted(InvertType.FollowMaster);
 
     /* Motor Encoders */
-    m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.PID_LOOP_IDX, DriveConstants.TIMEOUT_MS);
-    m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.PID_LOOP_IDX, DriveConstants.TIMEOUT_MS);
+    m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
+  
+    m_leftFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    m_leftFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    m_leftFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX);
+    m_leftFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kF, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kP, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kI, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kD, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX);
+    m_rightFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kF, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kP, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kI, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_GAINS.kD, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    m_leftFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
   }
 
   @Override