diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c5de68c..469fcb8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -58,7 +58,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.13, 0.0, 0.0, 0.053, 0, 1.0); + public static final Gains SHOOTER_GAINS = new Gains(0.425, 0.0005, 13, 0.05, 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 9c5eb38..8831be9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -91,7 +91,7 @@ public class RobotContainer { .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 8200)); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 4000)); /*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 3c937b2..2e42c11 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -35,6 +35,8 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.setInverted(true); setShooterGains(); + + m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); @@ -73,16 +75,16 @@ public class Shooter extends SubsystemBase { * @param targetVel Target velocity to run motor at */ public void runDrumShooterVelocityPID(double targetVel, double actualVel) { - velP = actualVel/targetVel; - double runSpeed = actualVel + (12000*velP); - if (runSpeed > targetVel) {runSpeed = targetVel;} + velP = actualVel/targetVel; //Percent of target + double runSpeed = actualVel + (12000*velP); //Ramp up equation + //if (runSpeed > targetVel) {runSpeed = targetVel;} SmartDashboard.putNumber("shoot", actualVel); - - if (actualVel < targetVel - 7000){ - m_shooterFalcon.set(TalonFXControlMode.Velocity, runSpeed); + runSpeed = runSpeed/targetVel; //Convert to percent + if (actualVel < targetVel - 1000){ + m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed); } else{ - m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); + m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID } } } \ No newline at end of file