diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2d8ad15..62e9d20 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -80,8 +80,10 @@ 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.4, 0.0005, 13, 0.05, 0, 1.0); + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0005, 13, 0.05, 0, 1.0); + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); + public static final double SHOOTER_TURRET_MIN = -1.0; public static final double ENCODER_TICKS_PER_REV = 2048; public static final double NEO_UNITS_PER_REV = 42; public static final double DEGREES_PER_ROT = 360; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1c6f0d8..cc1e52b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -137,11 +137,14 @@ public class RobotContainer { /* TEST shooter rotate PIDs */ new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(360, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(360))); + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(360, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(360))); + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(720, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + .whileHeld(new RunCommand(() -> m_robotShooter.runshooterRotatePID(720))); + /* TEST for both commands above */ } diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 527bfa4..a41a19b 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -27,7 +27,7 @@ public class Shooter extends SubsystemBase { public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); - public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; + public static Gains m_shooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; // Configure PID Controllers CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); @@ -106,15 +106,15 @@ public class Shooter extends SubsystemBase { } /* Angle Adjustment PID Control */ - public void runAngleAdjustPID(double targetAngle, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput) + public void runAngleAdjustPID(double targetAngle) { // Set PID Coefficients - m_angleAdjustPIDController.setP(kP); - m_angleAdjustPIDController.setI(kI); - m_angleAdjustPIDController.setD(kD); - m_angleAdjustPIDController.setIZone(kIz); - m_angleAdjustPIDController.setFF(kF); - m_angleAdjustPIDController.setOutputRange(kminOutput, kmaxOutput); + m_angleAdjustPIDController.setP(m_shooterGains.m_kP); + m_angleAdjustPIDController.setI(m_shooterGains.m_kI); + m_angleAdjustPIDController.setD(m_shooterGains.m_kD); + m_angleAdjustPIDController.setIZone(m_shooterGains.m_kIzone); + m_angleAdjustPIDController.setFF(m_shooterGains.m_kF); + m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterGains.m_kPeakOutput); // Convert input angle in degrees to rotations of the motor targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; @@ -123,15 +123,15 @@ public class Shooter extends SubsystemBase { } /* Rotate Shooter PID Control */ - public void runshooterRotatePID(double targetAngle, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput) + public void runshooterRotatePID(double targetAngle) { // Set PID Coefficients - m_shooterRotatePIDController.setP(kP); - m_shooterRotatePIDController.setI(kI); - m_shooterRotatePIDController.setD(kD); - m_shooterRotatePIDController.setIZone(kIz); - m_shooterRotatePIDController.setFF(kF); - m_shooterRotatePIDController.setOutputRange(kminOutput, kmaxOutput); + m_shooterRotatePIDController.setP(m_shooterGains.m_kP); + m_shooterRotatePIDController.setI(m_shooterGains.m_kI); + m_shooterRotatePIDController.setD(m_shooterGains.m_kD); + m_shooterRotatePIDController.setFF(m_shooterGains.m_kF); + m_shooterRotatePIDController.setIZone(m_shooterGains.m_kIzone); + m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterGains.m_kPeakOutput); // Convert input angle in degrees to rotations of the motor targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;