CONSTATS

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);
     }

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);
     }
   }
 

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)
+      );
   }
 }

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;

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.

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);

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);