From de4265be232a8524c6902b8b2718856f4c4eecbd Mon Sep 17 00:00:00 2001
From: aarav18 <57469559+aarav18@users.noreply.github.com>
Date: Sun, 19 Jan 2020 00:28:21 -0700
Subject: [PATCH] Added Shooter Subsystem PID Stuff

Initialized gains and added velocity PID method.
---
 .../java/frc4388/robot/subsystems/Shooter.java | 18 +++++++++++++++++-
 1 file changed, 17 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index eb1b6d3..934cbae 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -29,6 +29,14 @@ public class Shooter extends SubsystemBase {
     m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
 
     m_shooterFalcon.setInverted(false);
+    
+    setShooterGains();
+    
+    m_shooterFalcon.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    
+    int closedLoopTimeMs = 1;
+    m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
   }
 
   @Override
@@ -38,7 +46,7 @@ public class Shooter extends SubsystemBase {
 
   /**
    * Runs drum shooter motor.
-   * @param speed
+   * @param speed Speed to set the motor at
    */
   public void runDrumShooter(double speed) {
     m_shooterFalcon.set(speed);
@@ -54,4 +62,12 @@ public class Shooter extends SubsystemBase {
     m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
     m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
   }
+  /**
+   * Runs drum shooter velocity PID.
+   * @param falcon Motor to use
+   * @param targetVel Target velocity to run motor at
+   */
+  public void runDrumShooterVelocityPID(WPI_TalonFX falcon, double targetVel) {
+    falcon.set(TalonFXControlMode.Velocity, targetVel);
+  }
 }