feat(launcher): Added AimTable, refactored some code, and added index…

…er cocking

src/main/java/org/robolancers321/subsystems/launcher/AimTable.java

@@ -1,6 +1,13 @@
 /* (C) Robolancers 2024 */
 package org.robolancers321.subsystems.launcher;
 
+import static org.robolancers321.util.MathUtils.epsilonEquals;
+
+import java.util.LinkedHashMap;
+import java.util.List;
+import java.util.NavigableSet;
+import java.util.TreeSet;
+
 public class AimTable {
   public static class AimCharacteristic {
     public final double angle;
@@ -12,26 +19,25 @@ public AimCharacteristic(double angle, double rpm) {
     }
   }
 
-  private static class AimCharacteristicTableEntry {
-    public final double distance;
-    public final AimCharacteristic aimCharacteristic;
+  private AimCharacteristic lastAimCharacteristic;
 
-    public AimCharacteristicTableEntry(double distance, AimCharacteristic aimCharacteristic) {
-      this.distance = distance;
-      this.aimCharacteristic = aimCharacteristic;
-    }
-  }
+  private double lastDistance = 0;
+
+  private double kInterpolationCacheThreshold = 0.0;
 
   // TODO: tune
   public static AimCharacteristic kRetractedAimCharacteristic = new AimCharacteristic(0.0, 0.0);
   public static AimCharacteristic kMatingAimCharacteristic = new AimCharacteristic(0.0, 0.0);
   public static AimCharacteristic kAmpAimCharacteristic = new AimCharacteristic(0.0, 0.0);
 
   // TODO: tune
-  private static AimCharacteristicTableEntry[] table = {
-    new AimCharacteristicTableEntry(0.0, new AimCharacteristic(0.0, 0.0)),
-    new AimCharacteristicTableEntry(1.0, new AimCharacteristic(0.0, 0.0))
-  };
+  private static LinkedHashMap<Double, AimCharacteristic> table =
+      new LinkedHashMap<>() {
+        {
+          put(0.0, new AimCharacteristic(0.0, 0.0));
+          put(1.0, new AimCharacteristic(0.0, 0.0));
+        }
+      };
 
   private static double interpolate(
       double lowKey, double lowValue, double highKey, double highValue, double x) {
@@ -41,8 +47,28 @@ private static double interpolate(
   }
 
   public static AimCharacteristic getSpeakerAimCharacteristic(double distance) {
-    // TODO: write this correctly (not with the tree because it has edge cases at bounds)
+    List<Double> keys = table.keySet().stream().toList();
+    double lowerBound = keys.get(0);
+    double upperBound = keys.get(keys.size() - 1);
+
+    if ((distance < lowerBound)) distance = lowerBound;
+    else if (distance > upperBound) distance = upperBound;
+
+    NavigableSet<Double> values = new TreeSet<>(keys);
+    double lowKey = values.floor(distance);
+    double highKey = values.ceiling(distance);
+
+    return new AimCharacteristic(
+        interpolate(lowKey, table.get(lowKey).angle, highKey, table.get(highKey).angle, distance),
+        interpolate(lowKey, table.get(lowKey).rpm, highKey, table.get(highKey).rpm, distance));
+  }
+
+  public AimCharacteristic getLastAimCharacteristic(double distance) {
+    if (!epsilonEquals(lastDistance, distance, kInterpolationCacheThreshold)) {
+      lastAimCharacteristic = getSpeakerAimCharacteristic(lastDistance);
+      kInterpolationCacheThreshold = lastDistance;
+    }
 
-    return new AimCharacteristic(0.0, 0.0);
+    return lastAimCharacteristic;
   }
 }

src/main/java/org/robolancers321/subsystems/launcher/Indexer.java

@@ -149,7 +149,8 @@ public Command acceptHandoff(BooleanSupplier beamBreakStateSupplier) {
     return run(() -> this.setRPM(kHandoffRPM)).until(beamBreakStateSupplier).finallyDo(this::off);
   }
 
-  public Command reindex(BooleanSupplier beamBreakStateSupplier) {
+  // It's like cocking a gun, get your mind out of the gutter
+  public Command cock(BooleanSupplier beamBreakStateSupplier) {
     /*
      * TODO
      *
@@ -163,17 +164,23 @@ public Command reindex(BooleanSupplier beamBreakStateSupplier) {
      *
      */
 
-    return null;
+    return new SequentialCommandGroup(
+            new WaitUntilCommand(beamBreakStateSupplier),
+            run(() -> this.setRPM(kReindexRPM)).until(beamBreakStateSupplier),
+            run(() -> this.setRPM(-kReindexRPM))
+                .until(() -> !beamBreakStateSupplier.getAsBoolean()),
+            new WaitCommand(0.2))
+        .finallyDo(this::off);
   }
 
-  public Command outtake(BooleanSupplier beamBreakStateSupplier) {
+  public Command feed(BooleanSupplier beamBreakStateSupplier) {
     return new ParallelRaceGroup(
             run(() -> this.setRPM(kOuttakeRPM)),
             new SequentialCommandGroup(
-                new WaitUntilCommand(() -> beamBreakStateSupplier.getAsBoolean()),
+                new WaitUntilCommand(beamBreakStateSupplier),
                 new WaitUntilCommand(() -> !beamBreakStateSupplier.getAsBoolean())),
             new WaitCommand(
-                0.4) // TODO: just incase beam break fails, stop after some safe amount of time
+                0.4) // just incase beam break fails, stop after some safe amount of time
             )
         .finallyDo(this::off);
   }

src/main/java/org/robolancers321/subsystems/launcher/Launcher.java

@@ -1,6 +1,8 @@
 /* (C) Robolancers 2024 */
 package org.robolancers321.subsystems.launcher;
 
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
@@ -10,6 +12,8 @@
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import org.robolancers321.subsystems.launcher.AimTable.AimCharacteristic;
 
+import java.util.function.DoubleSupplier;
+
 public class Launcher extends SubsystemBase {
   /*
    * Singleton
@@ -27,6 +31,9 @@ public static Launcher getInstance() {
    * Constants
    */
 
+  private static final int kBeamBreakChannelPort = 1;
+
+  private static final double kDebounceTime = 0.05;
   // TODO: beam break(s)
 
   /*
@@ -37,36 +44,48 @@ public static Launcher getInstance() {
   public Indexer indexer;
   public Flywheel flywheel;
 
+  public AimTable aimTable;
+
+  public Debouncer beamBreakDebouncer = new Debouncer(kDebounceTime, Debouncer.DebounceType.kBoth);
+
+  public DigitalInput beamBreak;
+
   // TODO: beam break(s)
 
   private Launcher() {
     this.pivot = Pivot.getInstance();
     this.indexer = Indexer.getInstance();
     this.flywheel = Flywheel.getInstance();
+    this.aimTable = new AimTable();
+    this.beamBreak = new DigitalInput(kBeamBreakChannelPort);
+  }
+
+  public boolean getBeamBreakState() {
+    return beamBreakDebouncer.calculate(beamBreak.get());
   }
 
   public Command yeetAmp() {
     return new SequentialCommandGroup(
         pivot.aimAtAmp(),
         new ParallelCommandGroup(
             flywheel.yeetNoteAmp(),
-            indexer.outtake(() -> true), // TODO: pass in beam break state
+            indexer.feed(() -> !getBeamBreakState()),
             new WaitCommand(0.2)));
   }
 
-  // TODO: how to go about ths for active tracking
-  public Command yeetSpeaker(double distance) {
-    AimCharacteristic aimCharacteristic = AimTable.getSpeakerAimCharacteristic(distance);
+  public Command yeetSpeaker(DoubleSupplier distanceSupplier) {
+    AimCharacteristic aimCharacteristic = aimTable.getLastAimCharacteristic(distanceSupplier.getAsDouble());
 
     return new SequentialCommandGroup(
-        indexer.reindex(() -> true), // TODO: pass in beam break state
+        indexer.cock(this::getBeamBreakState),
         new ParallelRaceGroup(
             flywheel.yeetNoteSpeaker(() -> aimCharacteristic.rpm),
             new SequentialCommandGroup(
                 pivot.aimAtSpeaker(() -> aimCharacteristic.angle),
                 new WaitUntilCommand(
-                    flywheel::isRevved), // TODO: maybe put a timeout here to be safe
-                indexer.outtake(() -> true), // TODO: pass in beam break state
+                    flywheel::isRevved),
+                new WaitCommand(0.1),
+                indexer.feed(this::getBeamBreakState),
                 new WaitCommand(0.2))));
   }
 }