diff --git a/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java index b369a8e..05c6280 100644 --- a/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java +++ b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java @@ -47,6 +47,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return m_timer.get() > 1; + return m_ShooterSubsystem.returnCurrentSpeed() > 0.8; } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index f8a5e64..075f230 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -8,6 +8,7 @@ import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ShooterConstants; @@ -20,12 +21,17 @@ public class ShooterSubsystem extends SubsystemBase { private double m_topSpeed = 0; private double m_bottomSpeed = 0; + SlewRateLimiter m_topLimiter = new SlewRateLimiter(0.2); + SlewRateLimiter m_bottomLimiter = new SlewRateLimiter(0.2); + public ShooterSubsystem() { m_bottom.setIdleMode(IdleMode.kCoast); m_top.setIdleMode(IdleMode.kCoast); m_bottom.setInverted(true); m_top.setInverted(true); + + //m_bottom.getEncoder().setPositionConversionFactor(); } public void reset() { @@ -40,7 +46,9 @@ public void setShootingSpeed(ShootSpeed speed) { switch (speed) { case Shooting: m_topSpeed = ShooterConstants.kShooterSpeedTop; + m_topSpeed = m_topLimiter.calculate(m_topSpeed); m_bottomSpeed = ShooterConstants.kShooterSpeedBottom; + m_bottomSpeed = m_bottomLimiter.calculate(m_topSpeed); // System.out.println("shoot speed: " + ShooterConstants.kShooterSpeed); break; case Off: @@ -63,6 +71,7 @@ public void periodic() { m_bottom.set(m_bottomSpeed); m_top.set(m_topSpeed); + } public static enum ShootSpeed {