Skip to content

Commit

Permalink
Filtered auto chooser (#89)
Browse files Browse the repository at this point in the history
* Fancy auto gen, doesnt work yet because there is no way to remove a chooser or remove from a chooser...

* ChangeableChooser somewhat works

* Moving to shuffleboard, its still broke a lot of off by one problems

* Add ChangableSendableChooser

* Use ChangableSendableChooser in FilteredChooserGroup

* Cleanup and bugfixing

* Fix bad merge

* Cleanup, remove test code

* Cleanup, remove test code

* Cleanup, remove test code
  • Loading branch information
pordonj authored May 3, 2024
1 parent 75bdc2d commit 9109f9c
Show file tree
Hide file tree
Showing 5 changed files with 571 additions and 32 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,7 @@
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.Constants;
Expand All @@ -30,13 +28,11 @@
import frc.robot.shooter.commands.SpinUpAndShoot;
import frc.robot.undertaker.UndertakerSubsystem;
import frc.robot.util.AllianceUtil;
import frc.robot.util.FilteredChooserGroup;
import java.util.Collections;
import java.util.List;

public class PathPlannerAutoGenerator {

private SendableChooser<String> autoSelector;

private Field2d field;
private ShuffleboardTab tab;
private GenericEntry delay;
Expand All @@ -46,6 +42,8 @@ public class PathPlannerAutoGenerator {
private final UndertakerSubsystem undertaker;
private final LEDSubsystem led;
private Command ppAuto;
private String chooserResult = "none yet";
private FilteredChooserGroup chooserGroup;

public PathPlannerAutoGenerator(
DriveSubsystem drive,
Expand Down Expand Up @@ -78,23 +76,6 @@ public PathPlannerAutoGenerator(
AllianceUtil::isRedAlliance,
drive);

autoSelector = new SendableChooser<String>();
List<String> autos = AutoBuilder.getAllAutoNames();
Collections.sort(autos);

for (String s : autos) {
autoSelector.addOption(s, s);
}
autoSelector.setDefaultOption("SubC-MidDC", "SubC-MidDC");

autoSelector.onChange((cmd) -> {
if (cmd != null) {
setStartingPose(cmd);
ppAuto = getPathPlannerAuto(); // because we are composing later, we need to cook a fresh
// Command every time we select.
}
});

setupShuffleboard();
}

Expand All @@ -116,7 +97,29 @@ private void setStartingPose(String cmdName) {
private void setupShuffleboard() {
field = new Field2d();
tab = Shuffleboard.getTab("Auto");
tab.add("Auto Selection", autoSelector).withPosition(0, 0).withSize(2, 1);
List<String> autos = AutoBuilder.getAllAutoNames();
Collections.sort(autos);

chooserGroup = new FilteredChooserGroup(tab, "AutoLayer", 3, autos.toArray(new String[0]));
chooserGroup.onChange(s -> {
chooserResult = s;
if (autos.contains(s)) {
setStartingPose(s);
ppAuto = new PathPlannerAuto(s);
}
});

tab.addString("Loaded Auto", () -> {
if (ppAuto != null) {
return ppAuto.getName();
} else {
return "no auto initialized";
}
});
tab.addBoolean(
"Auto Matches Choices",
() -> (ppAuto != null && ppAuto.getName().equals(chooserResult)));

tab.add("Starting Pose", field).withPosition(0, 1).withSize(6, 4);
tab.addString("Alliance", () -> AllianceUtil.isRedAlliance() ? "Red" : "Blue")
.withPosition(6, 1);
Expand Down Expand Up @@ -166,14 +169,6 @@ private void registerCommands() {
NamedCommands.registerCommand("spinDown", new AutoSetTargetSpeed(shooter, 0, 0)); // non-blocking
}

private Command getPathPlannerAuto() {
String selected = autoSelector.getSelected();
if (selected == null || selected.equalsIgnoreCase("none")) {
return Commands.none();
}
return new PathPlannerAuto(selected);
}

public Command getAutoCommand() {
Command autoCmd = new SequentialCommandGroup(
// We always want to shoot the preloaded piece, and we want to shoot before the delay.
Expand All @@ -185,7 +180,9 @@ public Command getAutoCommand() {
new WaitCommand(delay.getDouble(0)),
ppAuto,
new AutoSetTargetSpeed(shooter, 0, 0));
ppAuto = getPathPlannerAuto(); // rebake pp auto... this might cause delay (bad)
if (AutoBuilder.getAllAutoNames().contains(chooserResult)) {
ppAuto = new PathPlannerAuto(chooserResult); // rebake pp auto... this might cause delay (bad)
}
return autoCmd;
}
}
263 changes: 263 additions & 0 deletions src/main/java/frc/robot/util/ChangableSendableChooser.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,263 @@
package frc.robot.util;

import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;

import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.LinkedHashMap;
import java.util.Map;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.locks.ReentrantLock;
import java.util.function.Consumer;

/**
* Basically a fork of SendableChooser that allows you to remove/clear the options.
*
* @param <V> The type of the values to be stored
*/
public class ChangableSendableChooser<V> implements Sendable, AutoCloseable {
/** The key for the default value. */
private static final String DEFAULT = "default";

/** The key for the selected option. */
private static final String SELECTED = "selected";

/** The key for the active option. */
private static final String ACTIVE = "active";

/** The key for the option array. */
private static final String OPTIONS = "options";

/** The key for the instance number. */
private static final String INSTANCE = ".instance";

/** A map linking strings to the objects they represent. */
private final Map<String, V> m_map = new LinkedHashMap<>();

private String m_defaultChoice = "";
private final int m_instance;
private String m_previousVal;
private Consumer<V> m_listener;
private static final AtomicInteger s_instances = new AtomicInteger();

/** Instantiates a {@link SendableChooser}. */
@SuppressWarnings("this-escape")
public ChangableSendableChooser() {
m_instance = s_instances.getAndIncrement();
SendableRegistry.add(this, "SendableChangableChooser", m_instance);
}

@Override
public void close() {
SendableRegistry.remove(this);
}

/**
* Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the
* object will appear as the given name.
*
* @param name the name of the option
* @param object the option
*/
public void addOption(String name, V object) {
m_map.put(name, object);
}

/**
* Removes the given name and its object from the list of options.
* If it was the default choice, the default choice reverts to its default (empty string).
* If it was the current selected option, the selected option changes to the default choice.
* @param name the name of the option to remove
*/
public void removeOption(String name) {
if (m_map.containsKey(name)) {
if (m_defaultChoice.equals(name)) {
m_defaultChoice = "";
}

if (m_selected.equals(name)) {
m_selected = m_defaultChoice;
}

m_map.remove(name);
}
}

/**
* Removes all options from the chooser.
* After this call, the default choice will revert to empty string.
* After this call, setting any value will trigger onChange, even if the value is the same as the value before clearing.
*/
public void clearOptions() {
m_defaultChoice = "";
m_previousVal = "";
m_selected = m_defaultChoice;

m_map.clear();
}

/**
* Replaces the current chooser options with the given options.
* After this call, the default choice will revert to empty string.
* @param options map of (names to options) to add
*/
public void setOptions(Map<String, V> options) {
requireNonNullParam(options, "options", "setOptions");

clearOptions();
m_map.putAll(options);
}

/**
* Replaces the current chooser options with the given options, then sets a default option.
* @param options map of (names to options) to add
* @param defaultName the name of the default option
* @param defaultObject the default option
*/
public void setOptions(Map<String, V> options, String defaultName, V defaultObject) {
requireNonNullParam(options, "options", "setOptions");
requireNonNullParam(defaultName, "defaultName", "setOptions");
requireNonNullParam(defaultObject, "defaultObject", "setOptions");

setOptions(options);
setDefaultOption(defaultName, defaultObject);
if (m_selected.equals("")) {
m_selected = defaultName;
}
}

/**
* Adds the given object to the list of options and marks it as the default. Functionally, this is
* very close to {@link #addOption(String, Object)} except that it will use this as the default
* option if none other is explicitly selected.
*
* @param name the name of the option
* @param object the option
*/
public void setDefaultOption(String name, V object) {
requireNonNullParam(name, "name", "setDefaultOption");

m_defaultChoice = name;
addOption(name, object);
m_selected = m_defaultChoice;
}

/**
* Returns the selected option. If there is none selected, it will return the default. If there is
* none selected and no default, then it will return {@code null}.
*
* @return the option selected
*/
public V getSelected() {
m_mutex.lock();
try {
if (m_selected != null) {
return m_map.get(m_selected);
} else {
return m_map.get(m_defaultChoice);
}
} finally {
m_mutex.unlock();
}
}

/**
* Bind a listener that's called when the selected value changes. Only one listener can be bound.
* Calling this function will replace the previous listener.
*
* @param listener The function to call that accepts the new value
*/
public void onChange(Consumer<V> listener) {
requireNonNullParam(listener, "listener", "onChange");
m_mutex.lock();
m_listener = listener;
m_mutex.unlock();
}

private String m_selected;
private final ReentrantLock m_mutex = new ReentrantLock();

@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("String Chooser");
builder.publishConstInteger(INSTANCE, m_instance);
builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null);
builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null);
builder.addStringProperty(
ACTIVE,
() -> {
m_mutex.lock();
try {
if (m_selected != null) {
return m_selected;
} else {
return m_defaultChoice;
}
} finally {
m_mutex.unlock();
}
},
null);
builder.addStringProperty(
SELECTED,
() -> {
// SELECTED maintains a getter to keep a proper state and fire onChange when items are removed
// if the final item is removed, this handles gracefully (gets empty string and no onChange call)
V choice;
Consumer<V> listener;
String setSelectedTo;
m_mutex.lock();
try {
if (m_selected != null) {
setSelectedTo = m_selected;
} else {
setSelectedTo = m_defaultChoice;
}
if (!setSelectedTo.equals(m_previousVal)
&& m_listener != null
&& m_map.containsKey(setSelectedTo)) {
choice = m_map.get(setSelectedTo);
listener = m_listener;
} else {
choice = null;
listener = null;
}
m_previousVal = setSelectedTo;
} finally {
m_mutex.unlock();
}
if (listener != null) {
listener.accept(choice);
}
return setSelectedTo;
},
val -> {
V choice;
Consumer<V> listener;
m_mutex.lock();
try {
m_selected = val;
// If dashboard loads with a selected that isn't there anymore, reset it
if (!m_map.containsKey(m_selected)) {
m_selected = m_defaultChoice;
}
if (!m_selected.equals(m_previousVal) && m_listener != null) {
choice = m_map.get(val);
listener = m_listener;
} else {
choice = null;
listener = null;
}
m_previousVal = val;
} finally {
m_mutex.unlock();
}
if (listener != null) {
listener.accept(choice);
}
});
}
}
Loading

0 comments on commit 9109f9c

Please sign in to comment.