diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0a5502a..8852e44 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -77,6 +77,7 @@ public final class Constants { public static final class IntakeConstants { public static final int INTAKE_SPARK_ID = -9; public static final int EXTENDER_SPARK_ID = -10; + public static final double EXTENDER_SPEED = 0.3; } public static final class ShooterConstants { @@ -107,7 +108,10 @@ public final class Constants { public static final class StorageConstants { public static final int STORAGE_CAN_ID = -1; - + public static final double STORAGE_PARTIAL_BALL = 2; + public static final double STORAGE_FULL_BALL = 7; + public static final double STORAGE_SPEED = 0.5; + /* Ball Indexes */ public static final int BEAM_SENSOR_DIO_0 = 0; public static final int BEAM_SENSOR_DIO_1 = 1; @@ -123,7 +127,6 @@ public final class Constants { 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.2, 0.0, 0.0, 0.0, 0, 1.0); } diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java index 9576f35..7ac5f46 100644 --- a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java +++ b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java @@ -8,6 +8,7 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.StorageConstants; import frc4388.robot.subsystems.Storage; public class StorageIntakeFinal extends CommandBase { @@ -29,7 +30,7 @@ public class StorageIntakeFinal extends CommandBase { @Override public void execute() { if (m_storage.getBeam(1)){ - m_storage.setStoragePID(m_storage.getEncoderPos() + 5); + m_storage.setStoragePID(m_storage.getEncoderPos() + StorageConstants.STORAGE_FULL_BALL); } } diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java index 4b6cfee..4636edc 100644 --- a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java +++ b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java @@ -21,7 +21,8 @@ public class StorageIntakeGroup extends SequentialCommandGroup { public StorageIntakeGroup(Intake m_intake, Storage m_storage) { addCommands( new StoragePrepIntake(m_intake, m_storage), - new storageIntake(m_intake, m_storage), - new StorageIntakeFinal(m_storage)); + new StorageIntake(m_intake, m_storage), + new StorageIntakeFinal(m_storage) + ); } } diff --git a/src/main/java/frc4388/robot/commands/storageIntake.java b/src/main/java/frc4388/robot/commands/storageIntake.java index ee30708..30d73ce 100644 --- a/src/main/java/frc4388/robot/commands/storageIntake.java +++ b/src/main/java/frc4388/robot/commands/storageIntake.java @@ -8,16 +8,19 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.IntakeConstants; +import frc4388.robot.Constants.StorageConstants; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.Storage; -public class storageIntake extends CommandBase { +public class StorageIntake extends CommandBase { public Intake m_intake; public Storage m_storage; + public boolean intook; /** * Creates a new storageIntake. */ - public storageIntake(Intake inSub, Storage storeSub) { + public StorageIntake(Intake inSub, Storage storeSub) { m_intake = inSub; m_storage = storeSub; addRequirements(m_intake); @@ -27,6 +30,7 @@ public class storageIntake extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { + intook = false; } // Called every time the scheduler runs while the command is scheduled. @@ -34,11 +38,12 @@ public class storageIntake extends CommandBase { public void execute() { if (m_storage.getBeam(0)){ - m_storage.setStoragePID(m_storage.getEncoderPos() + 2); - m_intake.runExtender(-0.3); + m_storage.setStoragePID(m_storage.getEncoderPos() + StorageConstants.STORAGE_PARTIAL_BALL); + m_intake.runExtender(-IntakeConstants.EXTENDER_SPEED); + intook = true; } else{ - m_intake.runExtender(0.3); + m_intake.runExtender(IntakeConstants.EXTENDER_SPEED); } } @@ -50,7 +55,7 @@ public class storageIntake extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (m_storage.getBeam(0)){ + if (!m_storage.getBeam(0) && m_storage.getBeam(1) && intook){ return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/storageOutake.java b/src/main/java/frc4388/robot/commands/storageOutake.java index 48a28ba..c19ce5a 100644 --- a/src/main/java/frc4388/robot/commands/storageOutake.java +++ b/src/main/java/frc4388/robot/commands/storageOutake.java @@ -8,6 +8,7 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.StorageConstants; import frc4388.robot.subsystems.Storage; public class StorageOutake extends CommandBase { @@ -28,7 +29,7 @@ public class StorageOutake extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_storage.runStorage(-0.5); + m_storage.runStorage(-StorageConstants.STORAGE_SPEED); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/storagePrepAim.java b/src/main/java/frc4388/robot/commands/storagePrepAim.java index 1449ced..376a92b 100644 --- a/src/main/java/frc4388/robot/commands/storagePrepAim.java +++ b/src/main/java/frc4388/robot/commands/storagePrepAim.java @@ -8,6 +8,7 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.StorageConstants; import frc4388.robot.subsystems.Storage; public class StoragePrepAim extends CommandBase { @@ -29,7 +30,7 @@ public class StoragePrepAim extends CommandBase { @Override public void execute() { if (m_storage.getBeam(2) == false){ - m_storage.runStorage(0.5); + m_storage.runStorage(StorageConstants.STORAGE_SPEED); } else{ m_storage.runStorage(0); diff --git a/src/main/java/frc4388/robot/commands/storagePrepIntake.java b/src/main/java/frc4388/robot/commands/storagePrepIntake.java index 9b5b89e..3321027 100644 --- a/src/main/java/frc4388/robot/commands/storagePrepIntake.java +++ b/src/main/java/frc4388/robot/commands/storagePrepIntake.java @@ -8,6 +8,7 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.StorageConstants; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.Storage; @@ -34,7 +35,7 @@ public class StoragePrepIntake extends CommandBase { @Override public void execute() { if (m_storage.getBeam(1) == false){ - m_storage.runStorage(-0.5); + m_storage.runStorage(-StorageConstants.STORAGE_SPEED); } else{ m_storage.runStorage(0);