diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bec509c..7a92f68 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -111,7 +111,7 @@ public static final class IntakeConstants { public static final double kIntakeSpeed = 0.5; // TODO: Tune distance sensor threshold for detecting note - public static final double kDistanceSensorThreshold = 3; + public static final double kDistanceSensorThreshold = 10; } public static final class ShooterConstants { diff --git a/src/main/java/frc/robot/commands/IntakeArmPositionCommand.java b/src/main/java/frc/robot/commands/IntakeArmPositionCommand.java index 7349812..30ee44c 100644 --- a/src/main/java/frc/robot/commands/IntakeArmPositionCommand.java +++ b/src/main/java/frc/robot/commands/IntakeArmPositionCommand.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Robot; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.ArmPosition; @@ -29,6 +30,6 @@ public void initialize() { // Returns true when the command should end. @Override public boolean isFinished() { - return m_intakeSubsystem.armAtSetpoint(); + return m_intakeSubsystem.armAtSetpoint() || !Robot.isReal(); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index cbce3fe..daae545 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -30,14 +30,18 @@ public class IntakeSubsystem extends SubsystemBase { /** If true, the distance sensor will be used to determine if we have a note */ private boolean m_distanceSensorToggle = Robot.isReal(); - private Rev2mDistanceSensor m_distanceSensor = m_distanceSensorToggle ? new Rev2mDistanceSensor(Port.kMXP) : null; // NavX I2C access port + private Rev2mDistanceSensor m_distanceSensor = m_distanceSensorToggle ? new Rev2mDistanceSensor(Port.kMXP) : null; private double m_intakeSpeed = 0; private double m_armSetpoint = IntakeConstants.kIntakeRaisedAngle; /** Creates a new IntakeSubsystem */ public IntakeSubsystem() { - if (m_distanceSensorToggle) m_distanceSensor.setAutomaticMode(true); + if (m_distanceSensorToggle) { + m_distanceSensor.setAutomaticMode(true); + m_distanceSensor.setEnabled(true); + m_distanceSensor.setMeasurementPeriod(0.01); + } m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset); SmartDashboard.putNumber("arm", m_armEncoder.getAbsolutePosition()); @@ -124,7 +128,7 @@ public void toggleDistanceSensor() { @Override public void periodic() { - haveNote = m_distanceSensorToggle ? getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold : false; + haveNote = m_distanceSensorToggle ? (getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold && getDistanceSensor() > 0) : false; // Note: negative because encoder goes from 0 to -193 cuz weird double armMotorSpeed = MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3);