diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 096f898..8ff656b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -164,21 +164,24 @@ public final class Constants { public static final double STORAGE_SPEED = 0.5; public static final double STORAGE_TIMEOUT = 2000; + /* Storage Characteristics */ + public static final double MOTOR_ROTS_PER_STORAGE_ROT = 1; //For the first storage belt + public static final double INCHES_PER_STORAGE_ROT = 1; //Circumference of the first storage belt + /* Ball Indexes */ public static final int BEAM_SENSOR_SHOOTER = 1; public static final int BEAM_SENSOR_USELESS = 2; public static final int BEAM_SENSOR_STORAGE = 3; public static final int BEAM_SENSOR_INTAKE = 4; + /* PID Gains */ + public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + /* PID Values */ public static final int SLOT_DISTANCE = 0; /* PID Indexes */ public static final int PID_PRIMARY = 0; - - /* PID Gains */ - public static final double STORAGE_MIN_OUTPUT = -1.0; - public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); } public static final class PneumaticsConstants { diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java index 3709946..ea4aa19 100644 --- a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java +++ b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java @@ -18,6 +18,7 @@ public class ManageStorage extends CommandBase { /* Keeps track of which beam breaks are pressed */ boolean isBallInIntake = false; boolean isBallInStorage = false; + boolean isBallInUseless = false; boolean isBallInShooter = false; /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */ @@ -39,9 +40,10 @@ public class ManageStorage extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - isBallInIntake = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE); - isBallInStorage = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE); - isBallInShooter = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_SHOOTER); + isBallInIntake = !m_storage.getBeamIntake(); + isBallInStorage = !m_storage.getBeamStorage(); + isBallInUseless = !m_storage.getBeamUseless(); + isBallInShooter = !m_storage.getBeamShooter(); m_isStorageEmpty = !isBallInStorage; } @@ -49,13 +51,15 @@ public class ManageStorage extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - isBallInIntake = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE); - isBallInStorage = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE); - isBallInShooter = !m_storage.getBeam(StorageConstants.BEAM_SENSOR_SHOOTER); + isBallInIntake = !m_storage.getBeamIntake(); + isBallInStorage = !m_storage.getBeamStorage(); + isBallInUseless = !m_storage.getBeamUseless(); + isBallInShooter = !m_storage.getBeamShooter(); - SmartDashboard.putBoolean("Ball in Intake", isBallInIntake); - SmartDashboard.putBoolean("Ball in Storage", isBallInStorage); - SmartDashboard.putBoolean("Ball in Shooter", isBallInShooter); + /// TODO: Delete/Comment these when done + SmartDashboard.putBoolean("!Ball in Intake!", isBallInIntake); + SmartDashboard.putBoolean("!Ball Storage!", isBallInStorage); + SmartDashboard.putBoolean("!Ball Shooter!", isBallInShooter); if (m_storageMode == StorageMode.IDLE) { runIdle(); diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java index 09850f8..00c9bb5 100644 --- a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java +++ b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java @@ -33,7 +33,7 @@ public class StoragePrep extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (m_storage.getBeam(1)){ + if (m_storage.getBeamShooter()){ //m_storage.runStorage(StorageConstants.STORAGE_SPEED); } else{ diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 4caa519..175e499 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -21,7 +21,10 @@ import frc4388.utility.Gains; public class Storage extends SubsystemBase { public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); - private DigitalInput[] m_beamSensors = new DigitalInput[6]; + private DigitalInput m_beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); + private DigitalInput m_beamUseless = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS); + private DigitalInput m_beamStorage = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE); + private DigitalInput m_beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); CANPIDController m_storagePIDController = m_storageMotor.getPIDController(); @@ -38,10 +41,14 @@ public class Storage extends SubsystemBase { */ public Storage() { resetEncoder(); - m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); - m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS); - m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE); - m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); + + // Set PID Coefficients + 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(storageGains.m_kminOutput, storageGains.m_kmaxOutput); } @Override @@ -55,9 +62,8 @@ public class Storage extends SubsystemBase { /** * Runs storage motor * - * @param input the voltage to run motor at + * @param input the percent output to run motor at */ - public void runStorage(double input) { m_storageMotor.set(input); } @@ -66,36 +72,58 @@ public class Storage extends SubsystemBase { m_encoder.setPosition(0); } - public void testBeams(){ - SmartDashboard.putBoolean("Beam 0", m_beamSensors[0].get()); - SmartDashboard.putBoolean("Beam 1", m_beamSensors[1].get()); - } - - /* Storage PID Control */ + /** + * Runs Storage to a particular position + * @param targetPos in inches + */ public void runStoragePositionPID(double targetPos){ - // Set PID Coefficients - 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_kmaxOutput); - //SmartDashboard.putNumber("Storage Position PID Target", targetPos); //SmartDashboard.putNumber("Storage Position Pos", getEncoderPos()); + targetPos = InchesToMotorRots(targetPos); m_storagePIDController.setReference(targetPos, ControlType.kPosition); } + /** + * Runs Storage to a particular position + * @param position in motor rotations + */ + public void setStoragePID(double position){ + m_storagePIDController.setReference(position, ControlType.kPosition); + } public double getEncoderPos(){ return m_encoder.getPosition(); } - public boolean getBeam(int id){ - return m_beamSensors[id].get(); + /** + * @param motorRots + * @return inches + */ + public double motorRotsToInches(double motorRots) { + return motorRots * (1/StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT) * (StorageConstants.INCHES_PER_STORAGE_ROT); } - public void setStoragePID(double position){ - m_storagePIDController.setReference(position , ControlType.kPosition); + /** + * @param inches + * @return motorRots + */ + public double InchesToMotorRots(double inches) { + return inches * (1/StorageConstants.INCHES_PER_STORAGE_ROT) * (StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT); + } + + public boolean getBeamShooter(){ + return m_beamShooter.get(); + } + + public boolean getBeamUseless(){ + return m_beamUseless.get(); + } + + public boolean getBeamStorage(){ + return m_beamStorage.get(); + } + + public boolean getBeamIntake(){ + return m_beamIntake.get(); } } diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java index f1b78f9..cfbb689 100644 --- a/src/main/java/frc4388/utility/Gains.java +++ b/src/main/java/frc4388/utility/Gains.java @@ -27,7 +27,7 @@ public class Gains { * @param kD The D value. * @param kF The F value. * @param kIzone The zone of the I value. - * @param kPeakOutput The peak output setting the motors to run the gains at. + * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0. */ public Gains(double kP, double kI, double kD, double kF, int kIzone, double kPeakOutput) { @@ -37,5 +37,29 @@ public class Gains { m_kF = kF; m_kIzone = kIzone; m_kPeakOutput = kPeakOutput; + m_kmaxOutput = m_kPeakOutput; + m_kminOutput = -m_kPeakOutput; + } + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIzone The zone of the I value. + * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0. + * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0. + */ + public Gains(double kP, double kI, double kD, double kF, int kIzone, double kMinOutput, double kMaxOutput) + { + m_kP = kP; + m_kI = kI; + m_kD = kD; + m_kF = kF; + m_kIzone = kIzone; + m_kminOutput = kMinOutput; + m_kmaxOutput = kMaxOutput; + m_kPeakOutput = (Math.abs(m_kminOutput) > Math.abs(m_kmaxOutput)) ? Math.abs(m_kminOutput) : Math.abs(m_kmaxOutput); } }