From f2c45942877819e1fc84e3c20a89b27e64925b6c Mon Sep 17 00:00:00 2001 From: pordonj Date: Sun, 14 Apr 2024 13:06:59 -0500 Subject: [PATCH 01/10] Fancy auto gen, doesnt work yet because there is no way to remove a chooser or remove from a chooser... --- .../generator/PathPlannerAutoGenerator.java | 9 ++ .../frc/robot/util/FilteredChooserGroup.java | 69 +++++++++++ .../robot/util/MultiPartStringFilterer.java | 77 ++++++++++++ .../util/MultiPartStringFiltererTest.java | 117 ++++++++++++++++++ 4 files changed, 272 insertions(+) create mode 100644 src/main/java/frc/robot/util/FilteredChooserGroup.java create mode 100644 src/main/java/frc/robot/util/MultiPartStringFilterer.java create mode 100644 src/test/java/util/MultiPartStringFiltererTest.java diff --git a/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java b/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java index 466a2be..66bc7ba 100644 --- a/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java +++ b/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java @@ -34,6 +34,7 @@ import frc.robot.shooter.commands.SpinUpAndShoot; import frc.robot.undertaker.UndertakerSubsystem; import frc.robot.util.AllianceUtil; +import frc.robot.util.FilteredChooserGroup; public class PathPlannerAutoGenerator { @@ -48,6 +49,7 @@ public class PathPlannerAutoGenerator { private final UndertakerSubsystem undertaker; private final LEDSubsystem led; private Command ppAuto; + private String chooserResult = "none yet"; public PathPlannerAutoGenerator(DriveSubsystem drive, ArmSubsystem arm, ShooterSubsystem shooter, UndertakerSubsystem undertaker, LEDSubsystem led) { this.arm = arm; @@ -76,6 +78,13 @@ public PathPlannerAutoGenerator(DriveSubsystem drive, ArmSubsystem arm, ShooterS autoSelector = new SendableChooser(); List autos = AutoBuilder.getAllAutoNames(); Collections.sort(autos); + + + FilteredChooserGroup chooserGroup = new FilteredChooserGroup(Shuffleboard.getTab("CHOOSERTEST"), "ChooserTest", 3, autos.toArray(new String[0])); + Shuffleboard.getTab("CHOOSERTEST").addString("Chooser Result", () -> chooserResult); + chooserGroup.onChange(s -> { + chooserResult = s; + }); for (String s : autos) { autoSelector.addOption(s, s); diff --git a/src/main/java/frc/robot/util/FilteredChooserGroup.java b/src/main/java/frc/robot/util/FilteredChooserGroup.java new file mode 100644 index 0000000..b1ab276 --- /dev/null +++ b/src/main/java/frc/robot/util/FilteredChooserGroup.java @@ -0,0 +1,69 @@ +package frc.robot.util; + +import java.util.function.Consumer; + +import edu.wpi.first.util.ErrorMessages; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; + +public class FilteredChooserGroup { + + private MultiPartStringFilterer mpsf; + private String name; + private ShuffleboardTab tab; + private SendableChooser[] choosers; + private Consumer listener; // listener.accept to call + + public FilteredChooserGroup(ShuffleboardTab tab, String name, int layers, String... strings) { + this.name = name; + this.tab = tab; + mpsf = new MultiPartStringFilterer(layers, strings); + choosers = new SendableChooser[layers]; + + populateChooser(0, mpsf.getStringsForLayer(0)); + } + + private void populateChooser(int layer, String... parts) { + if(choosers[layer] != null) { + choosers[layer].close(); + } + choosers[layer] = new SendableChooser(); + for(int i = 0; i < parts.length; i++) { + choosers[layer].addOption(parts[i], parts[i]); + } + + choosers[layer].onChange(s -> { + if((layer + 1) < mpsf.getLayerCount()) { + // populate next layer chooser + String[] previousParts = new String[layer]; + for(int i = 0; i < layer; i++) { + previousParts[i] = choosers[i].getSelected(); + } + populateChooser(layer + 1, mpsf.getStringsForLayer(layer + 1, previousParts)); + } + + // report an overall change + String[] allParts = new String[mpsf.getLayerCount()]; + for(int i = 0; i < layer; i++) { + allParts[i] = choosers[i].getSelected(); + } + listener.accept(MultiPartStringFilterer.assembleParts(allParts)); + }); + + tab.add(name + layer, choosers[layer]); + + if((layer + 1) < mpsf.getLayerCount() && choosers[layer + 1] == null) { + // populate next layer chooser + String[] previousParts = new String[layer]; + for(int i = 0; i < layer; i++) { + previousParts[i] = choosers[i].getSelected(); + } + populateChooser(layer + 1, mpsf.getStringsForLayer(layer + 1, previousParts)); + } + } + + public void onChange(Consumer listener) { + ErrorMessages.requireNonNullParam(listener, "listener", "onChange"); + this.listener = listener; + } +} diff --git a/src/main/java/frc/robot/util/MultiPartStringFilterer.java b/src/main/java/frc/robot/util/MultiPartStringFilterer.java new file mode 100644 index 0000000..81260d0 --- /dev/null +++ b/src/main/java/frc/robot/util/MultiPartStringFilterer.java @@ -0,0 +1,77 @@ +package frc.robot.util; + +import java.util.ArrayList; +import java.util.HashSet; +import java.util.LinkedHashSet; +import java.util.List; +import java.util.Set; + +/** + * Actively filters a list of strings by "layers" - separate string "parts" delimited by dashes + */ +public class MultiPartStringFilterer { + + private int layers; + private String[] fullStrings; + + public MultiPartStringFilterer(int layers, String[] strings) { + this.layers = layers; + this.fullStrings = strings; + } + + public int getLayerCount() { + return layers; + } + + public String[] getStringsForLayer(int layer, String... previousStringParts) { + Set returnSet = new LinkedHashSet(); + // first - if we dont have a number of previous strings equal to the requested layer, return an empty array. + if(previousStringParts.length < layer) { + return new String[0]; + } + + // assemble previousStrings into what prospective parts would start with + String starter = assembleParts(previousStringParts); + if (starter.length() > 0) starter += "-"; + + for(int i = 0; i < fullStrings.length; i++) { + if(fullStrings[i].startsWith(starter)) { + returnSet.add(getNextPart(fullStrings[i], starter, layer)); + } + } + + return returnSet.toArray(new String[0]); + } + + public static String assembleParts(String... stringParts) { + StringBuilder returnBuilder = new StringBuilder(); + for(int i = 0; i < stringParts.length; i++) + { + if(i != 0) + { + returnBuilder.append("-"); + } + returnBuilder.append(stringParts[i]); + } + + return returnBuilder.toString(); + } + + public String getNextPart(String fullString, String starter, int layersDeep) { + String nextPart = ""; + + if(layersDeep < layers && fullString.startsWith(starter)) + { + if(layers - layersDeep == 1) { + // final layer, return rest of string + nextPart = fullString.substring(starter.length()); + } else { + // return from end of starter to next dash (or end of string) + int nextPartEnd = fullString.indexOf("-", starter.length()); + nextPart = fullString.substring(starter.length(), nextPartEnd != -1 ? nextPartEnd : fullString.length()); + } + } + + return nextPart; + } +} diff --git a/src/test/java/util/MultiPartStringFiltererTest.java b/src/test/java/util/MultiPartStringFiltererTest.java new file mode 100644 index 0000000..08aa81c --- /dev/null +++ b/src/test/java/util/MultiPartStringFiltererTest.java @@ -0,0 +1,117 @@ +package util; + +import java.util.ArrayList; +import java.util.Arrays; + +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; + +import frc.robot.util.MultiPartStringFilterer; + +public class MultiPartStringFiltererTest { + + @Test + public void assembleParts() { + String test1 = MultiPartStringFilterer.assembleParts("SubA", "CloseABC", "MidCD"); + Assertions.assertEquals("SubA-CloseABC-MidCD", test1); + + // assembling nothing returns blank string + String test2 = MultiPartStringFilterer.assembleParts(); + Assertions.assertEquals("", test2); + + // 0-length parts are fine + String test3 = MultiPartStringFilterer.assembleParts("SubA", "", "MidCD"); + Assertions.assertEquals("SubA--MidCD", test3); + } + + @Test + public void getNextPart_normalUsage() { + String[] fullStrings = new String[0]; + + MultiPartStringFilterer sut = new MultiPartStringFilterer(3, fullStrings); + + // on the first layer, return the first part. + String test1 = sut.getNextPart("SubA-CloseABC-MidCD-FarA", "", 0); + Assertions.assertEquals("SubA", test1); + + String test2 = sut.getNextPart("SubA-CloseABC-MidCD-FarA", "SubA-", 1); + Assertions.assertEquals("CloseABC", test2); + + // on the final layer, return the rest of the string even if there are dashes. + String test3 = sut.getNextPart("SubA-CloseABC-MidCD-FarA", "SubA-CloseABC-", 2); + Assertions.assertEquals("MidCD-FarA", test3); + + // get a normal final layer + String test4 = sut.getNextPart("SubA-CloseABC-MidCD", "SubA-CloseABC-", 2); + Assertions.assertEquals("MidCD", test4); + + // get a final layer for something that was shorter than the max layers + String test5 = sut.getNextPart("SubA-CloseABC", "SubA-", 1); + Assertions.assertEquals("CloseABC", test5); + } + + @Test + public void getNextPart_exceptionalCases() { + String[] fullStrings = new String[0]; + + MultiPartStringFilterer sut = new MultiPartStringFilterer(3, fullStrings); + + // asking with layersDeep equal to or greater than the MPSF layers should return empty string + String test1 = sut.getNextPart("SubA-CloseABC-MidCD-FarA", "SubA-CloseABC-MidCD", 3); + Assertions.assertEquals("", test1); + + // asking with a starter that doesn't match should return empty string + String test2 = sut.getNextPart("SubA-CloseABC-MidCD-FarA", "SubB", 1); + Assertions.assertEquals("", test2); + + // don't freak out at 0-length parts + String test3 = sut.getNextPart("SubA--MidCD-FarA", "SubA", 1); + Assertions.assertEquals("", test3); + + // asking for a next part when there are no more parts, also not a big deal + String test4 = sut.getNextPart("SubA-CloseABC", "SubA-CloseABC", 2); + Assertions.assertEquals("", test4); + } + + @Test + public void getStringsForLayer() { + String[] fullStrings = new String[8]; + fullStrings[0] = "SubA-CloseABC-MidCD"; + fullStrings[1] = "SubA-CloseABC-MidCD-FarAB"; + fullStrings[2] = "SubA-CloseABC-MidDC"; + fullStrings[3] = "SubB-CloseABC-MidCD"; + fullStrings[4] = "SubB-CloseB"; + fullStrings[5] = "SubB-CloseCBA-MidCD"; + fullStrings[6] = "SubC--MidCD"; + fullStrings[7] = "SubC--MidDC"; + + MultiPartStringFilterer sut = new MultiPartStringFilterer(3, fullStrings); + + // layer 0 is all normal. Duplicates are not shown. + String[] test1 = sut.getStringsForLayer(0); + Assertions.assertArrayEquals(new String[]{"SubA", "SubB", "SubC"}, test1); + + // layer 1 for SubA is also normal. + String[] test2 = sut.getStringsForLayer(1, "SubA"); + Assertions.assertArrayEquals(new String[]{"CloseABC"}, test2); + + String[] test3 = sut.getStringsForLayer(2, "SubA", "CloseABC"); + Assertions.assertArrayEquals(new String[]{"MidCD", "MidCD-FarAB", "MidDC"}, test3); + + String[] test4 = sut.getStringsForLayer(1, "SubB"); + Assertions.assertArrayEquals(new String[]{"CloseABC", "CloseB", "CloseCBA"}, test4); + + String[] test5 = sut.getStringsForLayer(2, "SubB", "CloseABC"); + Assertions.assertArrayEquals(new String[]{"MidCD"}, test5); + + String[] test6 = sut.getStringsForLayer(2, "SubB", "CloseB"); + Assertions.assertArrayEquals(new String[]{}, test6); + + String[] test7 = sut.getStringsForLayer(1, "SubC"); + Assertions.assertArrayEquals(new String[]{""}, test7); + + String[] test8 = sut.getStringsForLayer(2, "SubC", ""); + Assertions.assertArrayEquals(new String[]{"MidCD", "MidDC"}, test8); + + } +} From 2b14a253dfaa1b384fd119e2602a5419be036a4e Mon Sep 17 00:00:00 2001 From: pordonj Date: Sun, 14 Apr 2024 22:29:17 -0500 Subject: [PATCH 02/10] ChangeableChooser somewhat works --- src/main/java/frc/robot/Robot.java | 12 +++ .../generator/PathPlannerAutoGenerator.java | 11 +-- .../java/frc/robot/util/ChangableChooser.java | 76 +++++++++++++++++++ .../frc/robot/util/FilteredChooserGroup.java | 4 +- 4 files changed, 95 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc/robot/util/ChangableChooser.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e77bd25..3f60663 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,23 +8,33 @@ import com.revrobotics.REVPhysicsSim; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.util.ChangableChooser; public class Robot extends TimedRobot { private Command m_autonomousCommand; private RobotContainer m_robotContainer; + private ChangableChooser newChooser; + @Override public void robotInit() { m_robotContainer = new RobotContainer(); FollowPathCommand.warmupCommand().schedule(); //Load all pathplanner classes in order to prevent delay when initally following path + + newChooser = new ChangableChooser("TEST_CHANGE_CHOOSER"); + newChooser.setOptions(new String[]{ "foo", "bar", "baz" }); + + Shuffleboard.getTab("CHOOSERTEST").addString("Chooser Result", () -> newChooser.get()); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + newChooser.periodic(); } @Override @@ -42,6 +52,7 @@ public void disabledExit() {} @Override public void autonomousInit() { + newChooser.setOptions(new String[]{ "autofoo", "autobar", "autobaz" }); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { @@ -57,6 +68,7 @@ public void autonomousExit() {} @Override public void teleopInit() { + newChooser.setOptions(new String[]{ "telefoo", "telebar", "telebaz" }); if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } diff --git a/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java b/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java index 66bc7ba..a0ed02c 100644 --- a/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java +++ b/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java @@ -34,6 +34,7 @@ import frc.robot.shooter.commands.SpinUpAndShoot; import frc.robot.undertaker.UndertakerSubsystem; import frc.robot.util.AllianceUtil; +import frc.robot.util.ChangableChooser; import frc.robot.util.FilteredChooserGroup; public class PathPlannerAutoGenerator { @@ -80,11 +81,11 @@ public PathPlannerAutoGenerator(DriveSubsystem drive, ArmSubsystem arm, ShooterS Collections.sort(autos); - FilteredChooserGroup chooserGroup = new FilteredChooserGroup(Shuffleboard.getTab("CHOOSERTEST"), "ChooserTest", 3, autos.toArray(new String[0])); - Shuffleboard.getTab("CHOOSERTEST").addString("Chooser Result", () -> chooserResult); - chooserGroup.onChange(s -> { - chooserResult = s; - }); + //FilteredChooserGroup chooserGroup = new FilteredChooserGroup(Shuffleboard.getTab("CHOOSERTEST"), "ChooserTest", 3, autos.toArray(new String[0])); + + // chooserGroup.onChange(s -> { + // chooserResult = s; + // }); for (String s : autos) { autoSelector.addOption(s, s); diff --git a/src/main/java/frc/robot/util/ChangableChooser.java b/src/main/java/frc/robot/util/ChangableChooser.java new file mode 100644 index 0000000..58502ab --- /dev/null +++ b/src/main/java/frc/robot/util/ChangableChooser.java @@ -0,0 +1,76 @@ +package frc.robot.util; + +import java.util.Arrays; + +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringArrayPublisher; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StringSubscriber; + +/** HEAVILY inspired by 6328's SwitchableChooser */ +public class ChangableChooser { + + private static final String placeholder = ""; + + private String[] options = new String[] {placeholder}; + private String active = placeholder; + + + private final StringPublisher namePublisher; + private final StringPublisher typePublisher; + private final StringArrayPublisher optionsPublisher; + private final StringPublisher defaultPublisher; + private final StringPublisher activePublisher; + private final StringPublisher selectedPublisher; + private StringSubscriber selectedInput; + + + public ChangableChooser(String name) { + var table = NetworkTableInstance.getDefault().getTable("SmartDashboard").getSubTable(name); + namePublisher = table.getStringTopic(".name").publish(); + typePublisher = table.getStringTopic(".type").publish(); + optionsPublisher = table.getStringArrayTopic("options").publish(); + defaultPublisher = table.getStringTopic("default").publish(); + activePublisher = table.getStringTopic("active").publish(); + selectedPublisher = table.getStringTopic("selected").publish(); + selectedInput = table.getStringTopic("selected").subscribe(this.options[0]); + + namePublisher.set(name); + typePublisher.set("String Chooser"); + optionsPublisher.set(this.options); + defaultPublisher.set(this.options[0]); + activePublisher.set(this.options[0]); + selectedPublisher.set(this.options[0]); + } + + /** Updates the set of available options. */ + public void setOptions(String[] options) { + if (Arrays.equals(options, this.options)) { + return; + } + this.options = options.length == 0 ? new String[] {placeholder} : options; + optionsPublisher.set(this.options); + periodic(); + } + + /** Returns the selected option. */ + public String get() { + return active == placeholder ? null : active; + } + + public void periodic() { + String selected = selectedInput.get(); + active = null; + for (String option : options) { + if (!option.equals(placeholder) && option.equals(selected)) { + active = option; + } + } + if (active == null) { + active = options[0]; + selectedPublisher.set(active); + } + defaultPublisher.set(active); + activePublisher.set(active); + } +} diff --git a/src/main/java/frc/robot/util/FilteredChooserGroup.java b/src/main/java/frc/robot/util/FilteredChooserGroup.java index b1ab276..130edc7 100644 --- a/src/main/java/frc/robot/util/FilteredChooserGroup.java +++ b/src/main/java/frc/robot/util/FilteredChooserGroup.java @@ -24,9 +24,7 @@ public FilteredChooserGroup(ShuffleboardTab tab, String name, int layers, String } private void populateChooser(int layer, String... parts) { - if(choosers[layer] != null) { - choosers[layer].close(); - } + choosers[layer] = new SendableChooser(); for(int i = 0; i < parts.length; i++) { choosers[layer].addOption(parts[i], parts[i]); From 3e86a5bb55ddb2f64dcc25e53eec9d538e3e7c93 Mon Sep 17 00:00:00 2001 From: pordonj Date: Sat, 20 Apr 2024 11:59:17 -0500 Subject: [PATCH 03/10] Moving to shuffleboard, its still broke a lot of off by one problems --- src/main/java/frc/robot/Robot.java | 11 +---- src/main/java/frc/robot/RobotContainer.java | 2 +- .../generator/PathPlannerAutoGenerator.java | 15 ++++--- .../java/frc/robot/util/ChangableChooser.java | 18 +++++++-- .../frc/robot/util/FilteredChooserGroup.java | 40 ++++++++++--------- 5 files changed, 48 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3f60663..711290e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -18,23 +18,16 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; - private ChangableChooser newChooser; - @Override public void robotInit() { m_robotContainer = new RobotContainer(); FollowPathCommand.warmupCommand().schedule(); //Load all pathplanner classes in order to prevent delay when initally following path - - newChooser = new ChangableChooser("TEST_CHANGE_CHOOSER"); - newChooser.setOptions(new String[]{ "foo", "bar", "baz" }); - - Shuffleboard.getTab("CHOOSERTEST").addString("Chooser Result", () -> newChooser.get()); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - newChooser.periodic(); + m_robotContainer.autoGenerator.periodic(); } @Override @@ -52,7 +45,6 @@ public void disabledExit() {} @Override public void autonomousInit() { - newChooser.setOptions(new String[]{ "autofoo", "autobar", "autobaz" }); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { @@ -68,7 +60,6 @@ public void autonomousExit() {} @Override public void teleopInit() { - newChooser.setOptions(new String[]{ "telefoo", "telebar", "telebaz" }); if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8417ac2..7731459 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -47,7 +47,7 @@ public class RobotContainer { //private final VisionSubsystem visionSubsystem; private final ArmSubsystem arm; - private final PathPlannerAutoGenerator autoGenerator; + public final PathPlannerAutoGenerator autoGenerator; private final RobotContext robotContext; private final CommandXboxController driverController; diff --git a/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java b/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java index a0ed02c..425c9b8 100644 --- a/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java +++ b/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java @@ -51,6 +51,7 @@ public class PathPlannerAutoGenerator { private final LEDSubsystem led; private Command ppAuto; private String chooserResult = "none yet"; + private FilteredChooserGroup chooserGroup; public PathPlannerAutoGenerator(DriveSubsystem drive, ArmSubsystem arm, ShooterSubsystem shooter, UndertakerSubsystem undertaker, LEDSubsystem led) { this.arm = arm; @@ -81,11 +82,11 @@ public PathPlannerAutoGenerator(DriveSubsystem drive, ArmSubsystem arm, ShooterS Collections.sort(autos); - //FilteredChooserGroup chooserGroup = new FilteredChooserGroup(Shuffleboard.getTab("CHOOSERTEST"), "ChooserTest", 3, autos.toArray(new String[0])); - - // chooserGroup.onChange(s -> { - // chooserResult = s; - // }); + chooserGroup = new FilteredChooserGroup("CHOOSERTEST", "ChooserTest", 3, autos.toArray(new String[0])); + Shuffleboard.getTab("CHOOSERTEST").addString("chooser result", () -> chooserResult); + chooserGroup.onChange(s -> { + chooserResult = s; + }); for (String s : autos) { autoSelector.addOption(s, s); @@ -102,6 +103,10 @@ public PathPlannerAutoGenerator(DriveSubsystem drive, ArmSubsystem arm, ShooterS setupShuffleboard(); } + public void periodic() { + chooserGroup.periodic(); + } + private void setStartingPose(String cmdName) { if (cmdName == null || cmdName.equals("None")) { field.setRobotPose(new Pose2d()); diff --git a/src/main/java/frc/robot/util/ChangableChooser.java b/src/main/java/frc/robot/util/ChangableChooser.java index 58502ab..17e5672 100644 --- a/src/main/java/frc/robot/util/ChangableChooser.java +++ b/src/main/java/frc/robot/util/ChangableChooser.java @@ -1,6 +1,8 @@ package frc.robot.util; import java.util.Arrays; +import java.util.function.Consumer; +import java.util.function.Supplier; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StringArrayPublisher; @@ -14,7 +16,8 @@ public class ChangableChooser { private String[] options = new String[] {placeholder}; private String active = placeholder; - + private String previousActive = placeholder; + private final StringPublisher namePublisher; private final StringPublisher typePublisher; @@ -24,9 +27,10 @@ public class ChangableChooser { private final StringPublisher selectedPublisher; private StringSubscriber selectedInput; + private Consumer changeLambda; - public ChangableChooser(String name) { - var table = NetworkTableInstance.getDefault().getTable("SmartDashboard").getSubTable(name); + public ChangableChooser(String tab, String name) { + var table = NetworkTableInstance.getDefault().getTable("/Shuffleboard").getSubTable(tab).getSubTable(name); namePublisher = table.getStringTopic(".name").publish(); typePublisher = table.getStringTopic(".type").publish(); optionsPublisher = table.getStringArrayTopic("options").publish(); @@ -72,5 +76,13 @@ public void periodic() { } defaultPublisher.set(active); activePublisher.set(active); + if(previousActive != active && changeLambda != null) { + changeLambda.accept(active); + } + previousActive = active; + } + + public void onChange(Consumer changeLambda) { + this.changeLambda = changeLambda; } } diff --git a/src/main/java/frc/robot/util/FilteredChooserGroup.java b/src/main/java/frc/robot/util/FilteredChooserGroup.java index 130edc7..adeebdb 100644 --- a/src/main/java/frc/robot/util/FilteredChooserGroup.java +++ b/src/main/java/frc/robot/util/FilteredChooserGroup.java @@ -4,38 +4,36 @@ import edu.wpi.first.util.ErrorMessages; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; public class FilteredChooserGroup { private MultiPartStringFilterer mpsf; private String name; - private ShuffleboardTab tab; - private SendableChooser[] choosers; + private String tabName; + private ChangableChooser[] choosers; private Consumer listener; // listener.accept to call - public FilteredChooserGroup(ShuffleboardTab tab, String name, int layers, String... strings) { + public FilteredChooserGroup(String tabName, String name, int layers, String... strings) { this.name = name; - this.tab = tab; + this.tabName = tabName; mpsf = new MultiPartStringFilterer(layers, strings); - choosers = new SendableChooser[layers]; + choosers = new ChangableChooser[layers]; populateChooser(0, mpsf.getStringsForLayer(0)); } private void populateChooser(int layer, String... parts) { - choosers[layer] = new SendableChooser(); - for(int i = 0; i < parts.length; i++) { - choosers[layer].addOption(parts[i], parts[i]); - } + choosers[layer] = new ChangableChooser(tabName, "Layer " + layer); + + choosers[layer].setOptions(parts); choosers[layer].onChange(s -> { if((layer + 1) < mpsf.getLayerCount()) { // populate next layer chooser - String[] previousParts = new String[layer]; - for(int i = 0; i < layer; i++) { - previousParts[i] = choosers[i].getSelected(); + String[] previousParts = new String[layer+1]; + for(int i = 0; i <= layer; i++) { + previousParts[i] = choosers[i].get(); } populateChooser(layer + 1, mpsf.getStringsForLayer(layer + 1, previousParts)); } @@ -43,18 +41,16 @@ private void populateChooser(int layer, String... parts) { // report an overall change String[] allParts = new String[mpsf.getLayerCount()]; for(int i = 0; i < layer; i++) { - allParts[i] = choosers[i].getSelected(); + allParts[i] = choosers[i].get(); } listener.accept(MultiPartStringFilterer.assembleParts(allParts)); }); - tab.add(name + layer, choosers[layer]); - if((layer + 1) < mpsf.getLayerCount() && choosers[layer + 1] == null) { // populate next layer chooser - String[] previousParts = new String[layer]; - for(int i = 0; i < layer; i++) { - previousParts[i] = choosers[i].getSelected(); + String[] previousParts = new String[layer+1]; + for(int i = 0; i <= layer; i++) { + previousParts[i] = choosers[i].get(); } populateChooser(layer + 1, mpsf.getStringsForLayer(layer + 1, previousParts)); } @@ -64,4 +60,10 @@ public void onChange(Consumer listener) { ErrorMessages.requireNonNullParam(listener, "listener", "onChange"); this.listener = listener; } + + public void periodic() { + for(int i = 0; i < choosers.length; i++) { + choosers[i].periodic(); + } + } } From 42d94881e7dd001fac9bb410892267b4d4dd8063 Mon Sep 17 00:00:00 2001 From: pordonj Date: Sun, 21 Apr 2024 21:59:37 -0500 Subject: [PATCH 04/10] Add ChangableSendableChooser --- src/main/java/frc/robot/RobotContainer.java | 18 +- src/main/java/frc/robot/arm/ArmSubsystem.java | 53 ++++ .../generator/PathPlannerAutoGenerator.java | 2 +- .../robot/util/ChangableSendableChooser.java | 260 ++++++++++++++++++ 4 files changed, 328 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/util/ChangableSendableChooser.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0ab473d..ed6013c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,6 +34,7 @@ import frc.robot.util.Dashboards; import frc.robot.util.RobotContext; import frc.robot.util.VersionFile; +import java.util.LinkedHashMap; public class RobotContainer { private final PoseScheduler poseScheduler; @@ -94,10 +95,19 @@ private void configureBindings() { .getShooterSpeed()[1])); driverController.a().onTrue(new TurnToAngle(drive, AllianceUtil.isRedAlliance() ? 0 : 180)); - driverController - .b() - .onTrue(new TurnToAngle(drive, AllianceUtil.isRedAlliance() ? 150 : -30.5)); // TODO alliance switching - driverController.x().onTrue(new TurnToAngle(drive, AllianceUtil.isRedAlliance() ? 90 : -90)); + driverController.b().onTrue(arm.testClearOptions()); // TODO alliance switching + + LinkedHashMap map1 = new LinkedHashMap<>(); + map1.put("foo1", "foo1val"); + map1.put("bar1", "bar1val"); + map1.put("baz1", "baz1val"); + + LinkedHashMap map2 = new LinkedHashMap<>(); + map2.put("foo2", "foo2val"); + map2.put("bar2", "bar2val"); + map2.put("baz2", "baz2val"); + driverController.x().onTrue(arm.testSetOptions(map1, "foo1", "foo1Val")); + driverController.y().onTrue(arm.testSetOptions(map2, "foo2", "foo2Val")); operatorController.leftTrigger().onTrue(new MoveToPosition(arm, Constants.Arm.AMP_POSITION)); operatorController.rightTrigger().onTrue(new MoveToHome(arm)); diff --git a/src/main/java/frc/robot/arm/ArmSubsystem.java b/src/main/java/frc/robot/arm/ArmSubsystem.java index c887cfd..6e6b677 100644 --- a/src/main/java/frc/robot/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/arm/ArmSubsystem.java @@ -4,9 +4,13 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; +import frc.robot.util.ChangableSendableChooser; +import java.util.Map; public class ArmSubsystem extends SubsystemBase { private double targetAngle = Constants.Arm.HOME_POSITION; @@ -16,6 +20,8 @@ public class ArmSubsystem extends SubsystemBase { private IArmIO armIO; private TrapezoidProfile.Constraints profileConstraints; + private ChangableSendableChooser testChooser; + public static ArmSubsystem create() { return new ArmSubsystem(Robot.isReal() ? new RealArmIO() : new SimArmIO()); } @@ -71,6 +77,8 @@ public boolean isBroken() { return broken; } + private String changeResult = "none yet"; + private void initDashboard() { dashboard = Shuffleboard.getTab("Arm"); dashboard.addDouble("Goal Angle", () -> getTarget()); @@ -84,6 +92,51 @@ private void initDashboard() { dashboard.addBoolean("Left home switch: ", () -> armIO.getLeftHomeSwitch()); dashboard.addBoolean("Is Broken", () -> isBroken()); dashboard.add(pid); + + testChooser = new ChangableSendableChooser(); + + testChooser.setDefaultOption("foo", "fooval"); + testChooser.addOption("bar", "barval"); + testChooser.addOption("baz", "bazval"); + + testChooser.onChange((s) -> { + changeResult = s; + }); + + dashboard.add("test chooser", testChooser); + dashboard.addString("test chooser selected", () -> { + String result = testChooser.getSelected(); + return result == null ? "" : result; + }); + dashboard.addString("test change result", () -> changeResult); + } + + public Command testRemoveOption() { + return new InstantCommand(() -> testChooser.removeOption("bar")); + } + + public Command testAddOption() { + return new InstantCommand(() -> { + testChooser.addOption("bang", "bangval"); + }); + } + + public Command testClearOptions() { + return new InstantCommand(() -> { + testChooser.clearOptions(); + }); + } + + public Command testSetOptions(Map options) { + return new InstantCommand(() -> { + testChooser.setOptions(options); + }); + } + + public Command testSetOptions(Map options, String dName, String dVal) { + return new InstantCommand(() -> { + testChooser.setOptions(options, dName, dVal); + }); } public double getPositionSetpoint() { diff --git a/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java b/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java index 8cee0dd..8f57329 100644 --- a/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java +++ b/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java @@ -104,7 +104,7 @@ public PathPlannerAutoGenerator( } }); - setupShuffleboard(); + // setupShuffleboard(); } public void periodic() { diff --git a/src/main/java/frc/robot/util/ChangableSendableChooser.java b/src/main/java/frc/robot/util/ChangableSendableChooser.java new file mode 100644 index 0000000..6f77b49 --- /dev/null +++ b/src/main/java/frc/robot/util/ChangableSendableChooser.java @@ -0,0 +1,260 @@ +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 The type of the values to be stored + */ +public class ChangableSendableChooser 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 m_map = new LinkedHashMap<>(); + + private String m_defaultChoice = ""; + private final int m_instance; + private String m_previousVal; + private Consumer 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. + */ + public void clearOptions() { + m_defaultChoice = ""; + 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 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 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); + } + + /** + * 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 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 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 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); + } + }); + } +} From 3fbf0898ba3cc8312b22e18d52f31892b5ad0c1a Mon Sep 17 00:00:00 2001 From: pordonj Date: Sat, 27 Apr 2024 21:13:54 -0500 Subject: [PATCH 05/10] Use ChangableSendableChooser in FilteredChooserGroup --- src/main/java/frc/robot/Robot.java | 1 - .../generator/PathPlannerAutoGenerator.java | 53 +++++------ .../robot/util/ChangableSendableChooser.java | 3 + .../frc/robot/util/FilteredChooserGroup.java | 90 ++++++++++++------- .../robot/util/MultiPartStringFilterer.java | 8 +- 5 files changed, 90 insertions(+), 65 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2520f01..7fd980a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -26,7 +26,6 @@ public void robotInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - m_robotContainer.autoGenerator.periodic(); } @Override diff --git a/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java b/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java index 8f57329..0d10a01 100644 --- a/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java +++ b/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java @@ -81,34 +81,7 @@ public PathPlannerAutoGenerator( AllianceUtil::isRedAlliance, drive); - autoSelector = new SendableChooser(); - List autos = AutoBuilder.getAllAutoNames(); - Collections.sort(autos); - - chooserGroup = new FilteredChooserGroup("CHOOSERTEST", "ChooserTest", 3, autos.toArray(new String[0])); - Shuffleboard.getTab("CHOOSERTEST").addString("chooser result", () -> chooserResult); - chooserGroup.onChange(s -> { - chooserResult = s; - }); - - 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(); - } - - public void periodic() { - chooserGroup.periodic(); + setupShuffleboard(); } private void setStartingPose(String cmdName) { @@ -129,7 +102,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 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); diff --git a/src/main/java/frc/robot/util/ChangableSendableChooser.java b/src/main/java/frc/robot/util/ChangableSendableChooser.java index 6f77b49..3f492e6 100644 --- a/src/main/java/frc/robot/util/ChangableSendableChooser.java +++ b/src/main/java/frc/robot/util/ChangableSendableChooser.java @@ -89,9 +89,11 @@ public void removeOption(String 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(); @@ -140,6 +142,7 @@ public void setDefaultOption(String name, V object) { m_defaultChoice = name; addOption(name, object); + m_selected = m_defaultChoice; } /** diff --git a/src/main/java/frc/robot/util/FilteredChooserGroup.java b/src/main/java/frc/robot/util/FilteredChooserGroup.java index edae2d0..28d5d50 100644 --- a/src/main/java/frc/robot/util/FilteredChooserGroup.java +++ b/src/main/java/frc/robot/util/FilteredChooserGroup.java @@ -1,67 +1,93 @@ package frc.robot.util; import edu.wpi.first.util.ErrorMessages; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import java.util.LinkedHashMap; import java.util.function.Consumer; public class FilteredChooserGroup { private MultiPartStringFilterer mpsf; private String name; - private String tabName; - private ChangableChooser[] choosers; + private ShuffleboardTab tab; + private ChangableSendableChooser[] choosers; private Consumer listener; // listener.accept to call - public FilteredChooserGroup(String tabName, String name, int layers, String... strings) { + public FilteredChooserGroup(ShuffleboardTab tab, String name, int layers, String... strings) { this.name = name; - this.tabName = tabName; + this.tab = tab; mpsf = new MultiPartStringFilterer(layers, strings); - choosers = new ChangableChooser[layers]; + choosers = new ChangableSendableChooser[layers]; + + // Initialize all choosers + initChoosers(); populateChooser(0, mpsf.getStringsForLayer(0)); } - private void populateChooser(int layer, String... parts) { + private void initChoosers() { + for (int layerToInit = 0; layerToInit < mpsf.getLayerCount(); layerToInit++) { + // need to make layer final for the loop so we can refer to it in change trigger + final int layer = layerToInit; + + choosers[layer] = new ChangableSendableChooser<>(); - choosers[layer] = new ChangableChooser(tabName, "Layer " + layer); + // set up change trigger + choosers[layer].onChange(s -> { + // if the next layer exists (max layer is layercount - 1) + if ((layer + 1) < mpsf.getLayerCount()) { + // populate next layer chooser + // we need to construct the parts leading up to this chooser. + // if we are first layer (0), there will be 1 part, and so on + String[] previousParts = new String[layer + 1]; + for (int i = 0; i <= layer; i++) { + previousParts[i] = choosers[i].getSelected(); + } - choosers[layer].setOptions(parts); + // use those parts to get the choices for the next layer + String[] nextLayerOptions = mpsf.getStringsForLayer(layer + 1, previousParts); - choosers[layer].onChange(s -> { - if ((layer + 1) < mpsf.getLayerCount()) { - // populate next layer chooser - String[] previousParts = new String[layer + 1]; - for (int i = 0; i <= layer; i++) { - previousParts[i] = choosers[i].get(); + populateChooser(layer + 1, nextLayerOptions); } - populateChooser(layer + 1, mpsf.getStringsForLayer(layer + 1, previousParts)); - } - // report an overall change - String[] allParts = new String[mpsf.getLayerCount()]; - for (int i = 0; i < layer; i++) { - allParts[i] = choosers[i].get(); - } - listener.accept(MultiPartStringFilterer.assembleParts(allParts)); - }); + // report an overall change + String[] allParts = new String[mpsf.getLayerCount()]; + for (int i = 0; i < mpsf.getLayerCount(); i++) { + allParts[i] = choosers[i].getSelected(); + } + listener.accept(MultiPartStringFilterer.assembleParts(allParts)); + }); - if ((layer + 1) < mpsf.getLayerCount() && choosers[layer + 1] == null) { + tab.add(name + " " + layer, choosers[layer]); + } + } + + private void populateChooser(int layer, String... options) { + LinkedHashMap optionMap = new LinkedHashMap<>(); + for (int i = 0; i < options.length; i++) { + optionMap.put(options[i], options[i]); + } + + // replace option set + choosers[layer].setOptions(optionMap); + + if (options.length > 0) { + choosers[layer].setDefaultOption(options[0], options[0]); + } + + // if there is a chooser for the next layer + /*if ((layer + 1) < mpsf.getLayerCount() && choosers[layer + 1] == null) { // populate next layer chooser String[] previousParts = new String[layer + 1]; for (int i = 0; i <= layer; i++) { - previousParts[i] = choosers[i].get(); + previousParts[i] = choosers[i].getSelected(); } populateChooser(layer + 1, mpsf.getStringsForLayer(layer + 1, previousParts)); - } + }*/ } public void onChange(Consumer listener) { ErrorMessages.requireNonNullParam(listener, "listener", "onChange"); this.listener = listener; } - - public void periodic() { - for (int i = 0; i < choosers.length; i++) { - choosers[i].periodic(); - } - } } diff --git a/src/main/java/frc/robot/util/MultiPartStringFilterer.java b/src/main/java/frc/robot/util/MultiPartStringFilterer.java index 8276fe3..9b3e844 100644 --- a/src/main/java/frc/robot/util/MultiPartStringFilterer.java +++ b/src/main/java/frc/robot/util/MultiPartStringFilterer.java @@ -43,10 +43,12 @@ public String[] getStringsForLayer(int layer, String... previousStringParts) { public static String assembleParts(String... stringParts) { StringBuilder returnBuilder = new StringBuilder(); for (int i = 0; i < stringParts.length; i++) { - if (i != 0) { - returnBuilder.append("-"); + if (stringParts[i] != null) { + if (i != 0) { + returnBuilder.append("-"); + } + returnBuilder.append(stringParts[i]); } - returnBuilder.append(stringParts[i]); } return returnBuilder.toString(); From 2af1ad7abe50511ce404722e3c836765bad51f2c Mon Sep 17 00:00:00 2001 From: pordonj Date: Sun, 28 Apr 2024 08:42:18 -0500 Subject: [PATCH 06/10] Cleanup and bugfixing --- .../generator/PathPlannerAutoGenerator.java | 17 +--- .../java/frc/robot/util/ChangableChooser.java | 88 ------------------- 2 files changed, 3 insertions(+), 102 deletions(-) delete mode 100644 src/main/java/frc/robot/util/ChangableChooser.java diff --git a/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java b/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java index 0d10a01..30fb431 100644 --- a/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java +++ b/src/main/java/frc/robot/auto/generator/PathPlannerAutoGenerator.java @@ -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; @@ -35,9 +33,6 @@ import java.util.List; public class PathPlannerAutoGenerator { - - private SendableChooser autoSelector; - private Field2d field; private ShuffleboardTab tab; private GenericEntry delay; @@ -174,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. @@ -193,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; } } diff --git a/src/main/java/frc/robot/util/ChangableChooser.java b/src/main/java/frc/robot/util/ChangableChooser.java deleted file mode 100644 index 91e5153..0000000 --- a/src/main/java/frc/robot/util/ChangableChooser.java +++ /dev/null @@ -1,88 +0,0 @@ -package frc.robot.util; - -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringArrayPublisher; -import edu.wpi.first.networktables.StringPublisher; -import edu.wpi.first.networktables.StringSubscriber; -import java.util.Arrays; -import java.util.function.Consumer; - -/** HEAVILY inspired by 6328's SwitchableChooser */ -public class ChangableChooser { - - private static final String placeholder = ""; - - private String[] options = new String[] {placeholder}; - private String active = placeholder; - private String previousActive = placeholder; - - private final StringPublisher namePublisher; - private final StringPublisher typePublisher; - private final StringArrayPublisher optionsPublisher; - private final StringPublisher defaultPublisher; - private final StringPublisher activePublisher; - private final StringPublisher selectedPublisher; - private StringSubscriber selectedInput; - - private Consumer changeLambda; - - public ChangableChooser(String tab, String name) { - var table = NetworkTableInstance.getDefault() - .getTable("/Shuffleboard") - .getSubTable(tab) - .getSubTable(name); - namePublisher = table.getStringTopic(".name").publish(); - typePublisher = table.getStringTopic(".type").publish(); - optionsPublisher = table.getStringArrayTopic("options").publish(); - defaultPublisher = table.getStringTopic("default").publish(); - activePublisher = table.getStringTopic("active").publish(); - selectedPublisher = table.getStringTopic("selected").publish(); - selectedInput = table.getStringTopic("selected").subscribe(this.options[0]); - - namePublisher.set(name); - typePublisher.set("String Chooser"); - optionsPublisher.set(this.options); - defaultPublisher.set(this.options[0]); - activePublisher.set(this.options[0]); - selectedPublisher.set(this.options[0]); - } - - /** Updates the set of available options. */ - public void setOptions(String[] options) { - if (Arrays.equals(options, this.options)) { - return; - } - this.options = options.length == 0 ? new String[] {placeholder} : options; - optionsPublisher.set(this.options); - periodic(); - } - - /** Returns the selected option. */ - public String get() { - return active == placeholder ? null : active; - } - - public void periodic() { - String selected = selectedInput.get(); - active = null; - for (String option : options) { - if (!option.equals(placeholder) && option.equals(selected)) { - active = option; - } - } - if (active == null) { - active = options[0]; - selectedPublisher.set(active); - } - defaultPublisher.set(active); - activePublisher.set(active); - if (previousActive != active && changeLambda != null) { - changeLambda.accept(active); - } - previousActive = active; - } - - public void onChange(Consumer changeLambda) { - this.changeLambda = changeLambda; - } -} From b4c72114d435c1ddd11d3c1e47e452ceeb7128c6 Mon Sep 17 00:00:00 2001 From: pordonj Date: Thu, 2 May 2024 21:21:59 -0500 Subject: [PATCH 07/10] Fix bad merge --- src/main/java/frc/robot/RobotContainer.java | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e8f19be..8e4a811 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,8 +33,6 @@ import frc.robot.util.AllianceUtil; import frc.robot.util.Dashboards; import frc.robot.util.RobotContext; -import frc.robot.util.VersionFile; -import java.util.LinkedHashMap; public class RobotContainer { private final PoseScheduler poseScheduler; @@ -98,18 +96,6 @@ private void configureBindings() { driverController.a().onTrue(new TurnToAngle(drive, AllianceUtil.isRedAlliance() ? 0 : 180)); driverController.b().onTrue(arm.testClearOptions()); // TODO alliance switching - LinkedHashMap map1 = new LinkedHashMap<>(); - map1.put("foo1", "foo1val"); - map1.put("bar1", "bar1val"); - map1.put("baz1", "baz1val"); - - LinkedHashMap map2 = new LinkedHashMap<>(); - map2.put("foo2", "foo2val"); - map2.put("bar2", "bar2val"); - map2.put("baz2", "baz2val"); - driverController.x().onTrue(arm.testSetOptions(map1, "foo1", "foo1Val")); - driverController.y().onTrue(arm.testSetOptions(map2, "foo2", "foo2Val")); - operatorController.leftTrigger().onTrue(new MoveToPosition(arm, Constants.Arm.AMP_POSITION)); operatorController.rightTrigger().onTrue(new MoveToHome(arm)); From 0375f693628e33952497ece5205354557baf8436 Mon Sep 17 00:00:00 2001 From: pordonj Date: Thu, 2 May 2024 21:28:56 -0500 Subject: [PATCH 08/10] Cleanup, remove test code --- src/main/java/frc/robot/RobotContainer.java | 5 +- src/main/java/frc/robot/arm/ArmSubsystem.java | 48 ------------------- 2 files changed, 4 insertions(+), 49 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8e4a811..6e72c7e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -94,7 +94,10 @@ private void configureBindings() { .getShooterSpeed()[1])); driverController.a().onTrue(new TurnToAngle(drive, AllianceUtil.isRedAlliance() ? 0 : 180)); - driverController.b().onTrue(arm.testClearOptions()); // TODO alliance switching + driverController + .b() + .onTrue(new TurnToAngle(drive, AllianceUtil.isRedAlliance() ? 150 : -30.5)); // TODO alliance switching + driverController.x().onTrue(new TurnToAngle(drive, AllianceUtil.isRedAlliance() ? 90 : -90)); operatorController.leftTrigger().onTrue(new MoveToPosition(arm, Constants.Arm.AMP_POSITION)); operatorController.rightTrigger().onTrue(new MoveToHome(arm)); diff --git a/src/main/java/frc/robot/arm/ArmSubsystem.java b/src/main/java/frc/robot/arm/ArmSubsystem.java index 6e6b677..3c9eee9 100644 --- a/src/main/java/frc/robot/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/arm/ArmSubsystem.java @@ -4,13 +4,10 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; import frc.robot.util.ChangableSendableChooser; -import java.util.Map; public class ArmSubsystem extends SubsystemBase { private double targetAngle = Constants.Arm.HOME_POSITION; @@ -92,51 +89,6 @@ private void initDashboard() { dashboard.addBoolean("Left home switch: ", () -> armIO.getLeftHomeSwitch()); dashboard.addBoolean("Is Broken", () -> isBroken()); dashboard.add(pid); - - testChooser = new ChangableSendableChooser(); - - testChooser.setDefaultOption("foo", "fooval"); - testChooser.addOption("bar", "barval"); - testChooser.addOption("baz", "bazval"); - - testChooser.onChange((s) -> { - changeResult = s; - }); - - dashboard.add("test chooser", testChooser); - dashboard.addString("test chooser selected", () -> { - String result = testChooser.getSelected(); - return result == null ? "" : result; - }); - dashboard.addString("test change result", () -> changeResult); - } - - public Command testRemoveOption() { - return new InstantCommand(() -> testChooser.removeOption("bar")); - } - - public Command testAddOption() { - return new InstantCommand(() -> { - testChooser.addOption("bang", "bangval"); - }); - } - - public Command testClearOptions() { - return new InstantCommand(() -> { - testChooser.clearOptions(); - }); - } - - public Command testSetOptions(Map options) { - return new InstantCommand(() -> { - testChooser.setOptions(options); - }); - } - - public Command testSetOptions(Map options, String dName, String dVal) { - return new InstantCommand(() -> { - testChooser.setOptions(options, dName, dVal); - }); } public double getPositionSetpoint() { From aecc8ebbdff5333e817b3f9f23488bd8d3ce8b5d Mon Sep 17 00:00:00 2001 From: pordonj Date: Thu, 2 May 2024 21:29:46 -0500 Subject: [PATCH 09/10] Cleanup, remove test code --- src/main/java/frc/robot/arm/ArmSubsystem.java | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/java/frc/robot/arm/ArmSubsystem.java b/src/main/java/frc/robot/arm/ArmSubsystem.java index 3c9eee9..c887cfd 100644 --- a/src/main/java/frc/robot/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/arm/ArmSubsystem.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; -import frc.robot.util.ChangableSendableChooser; public class ArmSubsystem extends SubsystemBase { private double targetAngle = Constants.Arm.HOME_POSITION; @@ -17,8 +16,6 @@ public class ArmSubsystem extends SubsystemBase { private IArmIO armIO; private TrapezoidProfile.Constraints profileConstraints; - private ChangableSendableChooser testChooser; - public static ArmSubsystem create() { return new ArmSubsystem(Robot.isReal() ? new RealArmIO() : new SimArmIO()); } @@ -74,8 +71,6 @@ public boolean isBroken() { return broken; } - private String changeResult = "none yet"; - private void initDashboard() { dashboard = Shuffleboard.getTab("Arm"); dashboard.addDouble("Goal Angle", () -> getTarget()); From 69c3e51cf65fcc3a99eda9b94ee926987e734c0d Mon Sep 17 00:00:00 2001 From: pordonj Date: Thu, 2 May 2024 21:30:18 -0500 Subject: [PATCH 10/10] Cleanup, remove test code --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6e72c7e..9504ab6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,7 +43,7 @@ public class RobotContainer { // private final VisionSubsystem visionSubsystem; private final ArmSubsystem arm; - public final PathPlannerAutoGenerator autoGenerator; + private final PathPlannerAutoGenerator autoGenerator; private final RobotContext robotContext; private final CommandXboxController driverController;