dfgn

src/main/java/frc4388/robot/subsystems/Storage.java

@@ -7,31 +7,30 @@ package frc4388.robot.subsystems;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import com.revrobotics.CANSparkMax;
 import edu.wpi.first.wpilibj.DigitalInput;
+import com.revrobotics.ColorSensorV3;
 
 public class Storage extends SubsystemBase {
   public CANSparkMax m_storageMotor;
-  private DigitalInput m_beamShooter;
-  private DigitalInput m_beamIntake;
+  public ColorSensorV3 m_colorSensor;
+  
 
   /** Creates a new Storage. */
-  public Storage(CANSparkMax storageMotor, DigitalInput beamShooter, DigitalInput beamIntake) {
+  public Storage(CANSparkMax storageMotor, ColorSensorV3 colorSensor) {
     m_storageMotor = storageMotor;
-    m_beamShooter = beamShooter;
-    m_beamIntake = beamIntake;
+    m_colorSensor = colorSensor;
   }
 
   public Storage(CANSparkMax storageMotor) {
     m_storageMotor = storageMotor;
-    m_beamShooter = null;
-    m_beamIntake = null;
+    m_colorSensor = null;
   }
   /**
    * If The Beam Is Broken, Run Storage
    * If Else, Stop Running Storage
    */
   public void manageStorage() {
-    if (getBeamIntake()) runStorage(0.d);
-    else runStorage(1.d);
+    if (getColorBroken()) runStorage(0.d);
+    else runStorage(0.9);
   }
   
   /**
@@ -42,19 +41,19 @@ public class Storage extends SubsystemBase {
     m_storageMotor.set(input);
   }
   /**
-   * Gets The Beam State On The Shooter
+   * Gets the state of the colorsensor as a beam break
    * @return The State Of The Beam on the Shooter
    */
-  public boolean getBeamShooter(){
-    return m_beamShooter.get();//True if open
+  public boolean getColorBroken(){
+    return (getRed() || getBlue());
   }
 
-  /**
-   * Gets The Beam State Of The Intake
-   * @return The Beam State Of The Intake
-   */
-  public boolean getBeamIntake(){
-    return m_beamIntake.get(); //True if open
+  public boolean getRed(){
+    return (m_colorSensor.getRed() >= 200);
+  }
+
+  public boolean getBlue(){
+    return (m_colorSensor.getBlue() >= 200);
   }