add some tof stuff

Iron-Panthers · Nov 17, 2023 · 109a286 · 109a286
1 parent f47a927
commit 109a286
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -259,6 +259,9 @@ public static final class Intake {
     public static final double CUBE_STATOR_LIMIT = 69;
     public static final double GROUND_CUBE_STATOR_LIMIT = 60;
 
+    public static final double CUBE_TOF_DISTANCE = 15;
+    public static final double CONE_TOF_DISTANCE = 20;
+
     public static final class Ports {
       public static final int INTAKE_MOTOR_PORT = CAN.at(18, "intake motor");
       public static final int CONE_TOF_PORT = 0;

diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -7,6 +7,8 @@
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.playingwithfusion.TimeOfFlight;
+
 import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -21,12 +23,15 @@ public class IntakeSubsystem extends SubsystemBase {
   private LinearFilter filter;
   private double filterOutput;
   private boolean isCone;
-  // private final TimeOfFlight coneToF, cubeToF;
+  private final TimeOfFlight coneTOF, cubeTOF;
 
   /** Creates a new IntakeSubsystem. */
   public IntakeSubsystem() {
     intakeMotor = new TalonFX(Constants.Intake.Ports.INTAKE_MOTOR_PORT);
 
+    coneTOF = new TimeOfFlight(Constants.Intake.Ports.CONE_TOF_PORT);
+    cubeTOF = new TimeOfFlight(Constants.Intake.Ports.CUBE_TOF_PORT);
+
     intakeMotor.configFactoryDefault();
     intakeMotor.clearStickyFaults();
 
@@ -41,9 +46,6 @@ public IntakeSubsystem() {
 
     isCone = false;
 
-    // coneToF = new TimeOfFlight(Constants.Intake.Ports.CONE_TOF_PORT);
-    // cubeToF = new TimeOfFlight(Constants.Intake.Ports.CUBE_TOF_PORT);
-
     shuffleboard.addDouble(
         "Intake motor sensor position", () -> intakeMotor.getSelectedSensorPosition());
     shuffleboard.addString("Current mode", () -> currentIntakeMode.toString());
@@ -120,6 +122,22 @@ private double getFilterCalculatedValue() {
     return filter.calculate(intakeMotor.getStatorCurrent());
   }
 
+  public double getCubeTOFDistance() {
+    return cubeTOF.getRange() / 25.4;
+  }
+
+  public double getConeTOFDistance() {
+    return coneTOF.getRange() / 25.4;
+  }
+
+  public boolean isCubeIntaked() {
+    return getCubeTOFDistance() <= Constants.Intake.CUBE_TOF_DISTANCE;
+  }
+
+  public boolean isConeIntaked() {
+    return getConeTOFDistance() <= Constants.Intake.CONE_TOF_DISTANCE;
+  }
+
   @Override
   public void periodic() {
     filterOutput = filter.calculate(intakeMotor.getStatorCurrent());