fix?: distance sensor update and can simulate robot now

SaintsRobotics · Mar 22, 2024 · ffd36ea · ffd36ea
1 parent 520aba0
commit ffd36ea
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 5 deletions.
diff --git 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
@@ -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
@@ -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);