From 641002eff0c4b11e23d3c58822c03d0281e210bd Mon Sep 17 00:00:00 2001 From: Gitter499 Date: Mon, 29 Jan 2024 19:59:19 -0500 Subject: [PATCH] feat(launcher): Added AimTable, refactored some code, and added indexer cocking --- .../subsystems/launcher/AimTable.java | 54 ++++++++++++++----- .../subsystems/launcher/Indexer.java | 17 ++++-- .../subsystems/launcher/Launcher.java | 33 +++++++++--- 3 files changed, 78 insertions(+), 26 deletions(-) diff --git a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java index ac9734f..85b263a 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java +++ b/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,15 +19,11 @@ 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); @@ -28,10 +31,13 @@ public AimCharacteristicTableEntry(double distance, AimCharacteristic aimCharact 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 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 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 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; } } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index ba3cc22..3164f6e 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/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); } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java index e586e2d..1fef901 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java +++ b/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,12 +44,24 @@ 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() { @@ -50,23 +69,23 @@ public Command yeetAmp() { 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)))); } }