diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index e2e9d55e..d19f4734 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -15,7 +15,9 @@ public interface Gamepad { public interface Conveyor { int CONVEYOR_MOTOR_PORT = 10; - int SHOOTER_FEEDER_MOTOR_PORT = 9; + int SHOOTER_FEEDER_MOTOR_PORT = 11; + + int IR_SENSOR_PORT = 12; } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index f13cf4f8..b67be926 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -26,6 +26,7 @@ public interface Conveyor { SmartNumber GANDALF_AMP_SPEED = new SmartNumber("Conveyor/Gandalf Amp Speed", 0); SmartNumber SHOOTER_FEEDER_SPEED = new SmartNumber("Conveyor/Shooter Feeder Speed", -0.9); + SmartNumber DEBOUNCE_TIME = new SmartNumber("Conveyor/Debounce Time", 0.2); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/Conveyor/ConveyorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/Conveyor/ConveyorImpl.java index 51a28f57..fa3ec074 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/Conveyor/ConveyorImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/Conveyor/ConveyorImpl.java @@ -2,20 +2,42 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkLowLevel.MotorType; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.wpilibj.DigitalInput; + import static com.stuypulse.robot.constants.Ports.Conveyor.*; import static com.stuypulse.robot.constants.Settings.Conveyor.*; import static com.stuypulse.robot.constants.Motors.Conveyor.*; + + public class ConveyorImpl extends Conveyor { private final CANSparkMax gandalfMotor; private final CANSparkMax shooterFeederMotor; + private final DigitalInput IRSensor; + + private BStream isAtShooter; protected ConveyorImpl(){ gandalfMotor = new CANSparkMax(CONVEYOR_MOTOR_PORT, MotorType.kBrushless); shooterFeederMotor = new CANSparkMax(SHOOTER_FEEDER_MOTOR_PORT, MotorType.kBrushless); + IRSensor = new DigitalInput(IR_SENSOR_PORT); + GANDALF_MOTOR.configure(gandalfMotor); SHOOTER_FEEDER_MOTOR.configure(shooterFeederMotor); + + isAtShooter = + BStream.create(this::isLoaded) + .filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); + } + + + public boolean isLoaded() { + return !IRSensor.get(); } @Override