diff --git a/src/main/java/frc/robot/commands/IndexCommand.java b/src/main/java/frc/robot/commands/IndexCommand.java index 36a21d12..922be48c 100644 --- a/src/main/java/frc/robot/commands/IndexCommand.java +++ b/src/main/java/frc/robot/commands/IndexCommand.java @@ -40,13 +40,15 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (m_Indexer.getInchesFromSensor()>8) { + if (!m_Indexer.isNoteIn()) { intake.intakeYes(1); m_Indexer.on(); shooter.turnOff(); } else { intake.intakeOff(); - m_Indexer.off(); + if(!shootButtonSupplier.getAsBoolean()) { + m_Indexer.off(); + } shooter.shootThing(1); } if (shootButtonSupplier.getAsBoolean()) { diff --git a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java index 639e76fc..0b117b23 100644 --- a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java @@ -1,19 +1,24 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAnalogSensor; import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.SparkAnalogSensor.Mode; + import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.settings.Constants.IndexerConstants; public class IndexerSubsystem extends SubsystemBase { CANSparkMax m_IndexerMotor; + SparkAnalogSensor m_DistanceSensor; public IndexerSubsystem() { m_IndexerMotor = new CANSparkMax(IndexerConstants.INDEXER_MOTOR, MotorType.kBrushless); + m_DistanceSensor = m_IndexerMotor.getAnalog(Mode.kAbsolute); } - - public double getInchesFromSensor() { - return 0; + + public boolean isNoteIn() { + return m_DistanceSensor.getVoltage()>1; } public void on() {