From 0f5a6ef09ff1ee553e37392f6f1d065dcb9a656a Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 1 Feb 2020 16:07:59 -0700
Subject: [PATCH] PID tuning

P and F
---
 src/main/java/frc4388/robot/Constants.java       |  2 +-
 src/main/java/frc4388/robot/RobotContainer.java  |  2 +-
 .../java/frc4388/robot/subsystems/Shooter.java   | 16 ++++++++--------
 3 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 0e904eb..e0c6f24 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -42,7 +42,7 @@ public final class Constants {
         public static final int SHOOTER_SLOT_IDX = 0;
         public static final int SHOOTER_PID_LOOP_IDX = 1;
         public static final int SHOOTER_TIMEOUT_MS = 30;
-        public static final Gains SHOOTER_GAINS = new Gains(0.0, 0.0, 0.0, 0.0542, 0, 1.0);
+        public static final Gains SHOOTER_GAINS = new Gains(0.13, 0.0, 0.0, 0.053, 0, 1.0);
         
         public static final double ENCODER_TICKS_PER_REV = 2048;
     }
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 0025383..fea891a 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -79,7 +79,7 @@ public class RobotContainer {
             .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
 
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
-            .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 13200));
+            .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 8200));
 
         /*new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
             .whenPressed(new InstantCommand(null, m_robotDrive));*/
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index fdbc39e..3c937b2 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -21,8 +21,6 @@ public class Shooter extends SubsystemBase {
 
   public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
 
-  private double m_targetVel = 2300;
-
   public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS;
 
   double velP;
@@ -43,14 +41,11 @@ public class Shooter extends SubsystemBase {
     int closedLoopTimeMs = 1;
     m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
     m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
-
-    SmartDashboard.putNumber("Shooter Velocity Target", m_targetVel);
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    m_targetVel = SmartDashboard.getNumber("Shooter Velocity Target", m_targetVel);
     SmartDashboard.putNumber("Shooter Velocity", m_shooterFalcon.getSelectedSensorVelocity()*600/ShooterConstants.ENCODER_TICKS_PER_REV);
   }
 
@@ -79,10 +74,15 @@ public class Shooter extends SubsystemBase {
    */
   public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
     velP = actualVel/targetVel;
-    double runSpeed = actualVel + (20000*velP);
+    double runSpeed = actualVel + (12000*velP);
     if (runSpeed > targetVel) {runSpeed = targetVel;}
-    System.err.println(velP + " " + runSpeed);
     SmartDashboard.putNumber("shoot", actualVel);
-    m_shooterFalcon.set(TalonFXControlMode.Velocity, runSpeed);
+    
+    if (actualVel < targetVel - 7000){
+      m_shooterFalcon.set(TalonFXControlMode.Velocity, runSpeed);
+    }
+    else{
+      m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel);
+    }
   }
 }
\ No newline at end of file