diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bcd15f8..318ac48 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -108,13 +108,8 @@ public final class Constants { public static final int PID_PRIMARY = 0; /* PID Gains */ - public static final double storP = 0.1; - public static final double storI = 1e-4; - public static final double storD = 1.0; - public static final double storIz = 0.0; - public static final double storF = 0.0; - public static final double storkmaxOutput = 1.0; - public static final double storkminOutput = -1.0; + public static final double STORAGE_MIN_OUTPUT = -1.0; + public static final Gains STORAGE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2585fbc..bf4a506 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -137,7 +137,7 @@ public class RobotContainer { /* Storage Neo PID Test */ new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); + .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5))); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 5fbcd1c..be3f97f 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -19,7 +19,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj2.command.SubsystemBase; - +import frc4388.robot.Gains; import frc4388.robot.Constants.StorageConstants; public class Storage extends SubsystemBase { @@ -30,6 +30,8 @@ public class Storage extends SubsystemBase { CANEncoder m_encoder = m_storageMotor.getEncoder(); + Gains storageGains = StorageConstants.STORAGE_GAINS; + /** * Creates a new Storage. */ @@ -72,15 +74,15 @@ public class Storage extends SubsystemBase { } /* Storage PID Control */ - public void runStoragePositionPID(double targetPos, double kP, double kI, double kD, double kIz, double kF, double kmaxOutput, double kminOutput) + public void runStoragePositionPID(double targetPos) { // Set PID Coefficients - m_storagePIDController.setP(kP); - m_storagePIDController.setI(kI); - m_storagePIDController.setD(kD); - m_storagePIDController.setIZone(kIz); - m_storagePIDController.setFF(kF); - m_storagePIDController.setOutputRange(kminOutput, kmaxOutput); + m_storagePIDController.setP(storageGains.m_kP); + m_storagePIDController.setI(storageGains.m_kI); + m_storagePIDController.setD(storageGains.m_kD); + m_storagePIDController.setIZone(storageGains.m_kIzone); + m_storagePIDController.setFF(storageGains.m_kF); + m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kPeakOutput); m_storagePIDController.setReference(targetPos, ControlType.kPosition); } @@ -94,7 +96,7 @@ public class Storage extends SubsystemBase { * Prepares storage for shooting */ public void storageAim() { - + //runStoragePositionPID(targetPos, kP, kI, kD, kIz, kF, kmaxOutput, kminOutput); } /* *If shooting move storage motor until top sensor is tripped