From 43a454f9ded01eb4ee37234e5a8cac205184ea5a Mon Sep 17 00:00:00 2001
From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com>
Date: Sat, 1 Feb 2020 12:40:39 -0700
Subject: [PATCH] Runup

---
 src/main/java/frc4388/robot/RobotContainer.java       |  4 ++--
 .../robot/commands/ShooterVelocityControlPID.java     |  4 ++--
 src/main/java/frc4388/robot/subsystems/Shooter.java   | 11 +++++------
 3 files changed, 9 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index e9527ae..153c296 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -60,7 +60,7 @@ public class RobotContainer {
         m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
         // continually sends updates to the Blinkin LED controller to keep the lights on
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
-        m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter));
+        m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0.2), m_robotShooter));
     }
 
     /**
@@ -91,7 +91,7 @@ public class RobotContainer {
             .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000));
 
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
-            .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 3200));
+            .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 13200));
 
         /*new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON)
             .whenPressed(new InstantCommand(null, m_robotDrive));*/
diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
index 799163b..67b5cb1 100644
--- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
+++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java
@@ -31,8 +31,8 @@ public class ShooterVelocityControlPID extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    System.err.println(m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity());
-    m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity());
+    System.err.println(m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
+    m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getSelectedSensorVelocity());
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java
index 6cfd3be..fdbc39e 100644
--- a/src/main/java/frc4388/robot/subsystems/Shooter.java
+++ b/src/main/java/frc4388/robot/subsystems/Shooter.java
@@ -79,11 +79,10 @@ public class Shooter extends SubsystemBase {
    */
   public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
     velP = actualVel/targetVel;
-    if(velP < 0.1){
-      velP = 0.1;
-    }
-    double runSpeed = velP*(1-velP);
-    //System.err.println(runSpeed);
-    m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed/*ShooterConstants.ENCODER_TICKS_PER_REV/600*/);
+    double runSpeed = actualVel + (20000*velP);
+    if (runSpeed > targetVel) {runSpeed = targetVel;}
+    System.err.println(velP + " " + runSpeed);
+    SmartDashboard.putNumber("shoot", actualVel);
+    m_shooterFalcon.set(TalonFXControlMode.Velocity, runSpeed);
   }
 }
\ No newline at end of file