diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 8f78712..5823c96 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/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); }