mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 08:48:07 -06:00
Fix missing path file
Re-enable PhotonCamera version check Revert PathPlanner auto preloading Complete working copy of Shuffleboard command schedule
This commit is contained in:
nathanrsxtn
@@ -18,7 +18,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
|
||||
import frc4388.utility.Commander;
|
||||
import frc4388.utility.RobotTime;
|
||||
import frc4388.utility.Vector2D;
|
||||
|
||||
@@ -58,7 +57,6 @@ public class Robot extends TimedRobot {
|
||||
LOGGER.log(Level.FINE, "Logging Test 6/8");
|
||||
LOGGER.log(Level.FINER, "Logging Test 7/8");
|
||||
LOGGER.log(Level.FINEST, "Logging Test 8/8");
|
||||
Commander.initialize();
|
||||
// var path =
|
||||
// PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move
|
||||
// Forward.path").toFile());
|
||||
@@ -94,7 +92,6 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
m_robotTime.updateTimes();
|
||||
Commander.periodic();
|
||||
Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00);
|
||||
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
|
||||
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition);
|
||||
@@ -169,9 +166,7 @@ public class Robot extends TimedRobot {
|
||||
* This function is called periodically during autonomous.
|
||||
*/
|
||||
@Override
|
||||
public void autonomousPeriodic() {
|
||||
final int a = 1;
|
||||
}
|
||||
public void autonomousPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
|
||||
@@ -42,6 +42,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.StorageConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.commands.CommandSchedule;
|
||||
import frc4388.robot.commands.PathRecorder;
|
||||
import frc4388.robot.commands.RunCommandForTime;
|
||||
import frc4388.robot.commands.ShooterTuner;
|
||||
@@ -97,6 +98,7 @@ public class RobotContainer {
|
||||
private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive);
|
||||
|
||||
private final ShooterTuner m_shooterTuner = new ShooterTuner(m_robotBoomBoom);
|
||||
private final CommandSchedule m_commandSchedule = new CommandSchedule(13, 6, false);
|
||||
// Controllers
|
||||
private final static DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final static DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
@@ -122,9 +124,6 @@ public class RobotContainer {
|
||||
|
||||
private SendableChooser<SequentialCommandGroup> quickAutoChooser = new SendableChooser<>();
|
||||
|
||||
private final SequentialCommandGroup autoJMove1 = buildAuto(3.0, 3.0, "JMove1");
|
||||
private final SequentialCommandGroup autoJMove2 = buildAuto(3.0, 3.0, "JMove2");
|
||||
|
||||
/**
|
||||
* SmartDash
|
||||
* - Limelight cam X
|
||||
@@ -283,6 +282,7 @@ public class RobotContainer {
|
||||
);
|
||||
|
||||
SmartDashboard.putData("Shooter Tuner", m_shooterTuner);
|
||||
SmartDashboard.putData("Command Schedule", m_commandSchedule);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -628,11 +628,11 @@ public class RobotContainer {
|
||||
// ! PathPlanner Testing
|
||||
ParallelDeadlineGroup intakeWithPath1 = new ParallelDeadlineGroup(new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), 3.0, true),
|
||||
new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer),
|
||||
autoJMove1);
|
||||
buildAuto(3.0, 3.0, "JMove1"));
|
||||
|
||||
ParallelDeadlineGroup intakeWithPath2 = new ParallelDeadlineGroup(new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), 5.0, true),
|
||||
new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer),
|
||||
autoJMove2);
|
||||
buildAuto(3.0, 3.0, "JMove2"));
|
||||
|
||||
ParallelCommandGroup extendWhileTurretIsAiming = new ParallelCommandGroup(new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
|
||||
ParallelCommandGroup intakeWithPathAndTrackTarget = new ParallelCommandGroup(intakeWithPath1, weirdAutoShootingGroup2);
|
||||
|
||||
@@ -0,0 +1,192 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.lang.invoke.MethodHandle;
|
||||
import java.lang.invoke.MethodHandles;
|
||||
import java.lang.invoke.MethodHandles.Lookup;
|
||||
import java.lang.reflect.AccessibleObject;
|
||||
import java.lang.reflect.Field;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Collection;
|
||||
import java.util.HashMap;
|
||||
import java.util.HashSet;
|
||||
import java.util.LinkedHashMap;
|
||||
import java.util.List;
|
||||
import java.util.Map;
|
||||
import java.util.Objects;
|
||||
import java.util.function.BooleanSupplier;
|
||||
import java.util.function.Function;
|
||||
import java.util.logging.Level;
|
||||
import java.util.logging.Logger;
|
||||
|
||||
import com.diffplug.common.base.Errors;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardContainer;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import edu.wpi.first.wpilibj2.command.CommandGroupBase;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
|
||||
public final class CommandSchedule extends CommandBase {
|
||||
private static final Logger LOGGER = Logger.getLogger(CommandSchedule.class.getSimpleName());
|
||||
private MethodHandle sequentialCommandGroupCommandsMethod;
|
||||
private MethodHandle sequentialCommandGroupCurrentCommandIndexMethod;
|
||||
private MethodHandle parallelCommandGroupCommandsMethod;
|
||||
private MethodHandle parallelDeadlineGroupCommandsMethod;
|
||||
private MethodHandle parallelRaceGroupCommandsMethod;
|
||||
private ShuffleboardLayout root;
|
||||
private ShuffleboardLayout ungroupedLayout;
|
||||
private LinkedHashMap<Command, Object> scheduledCommands;
|
||||
private final int maxWidth;
|
||||
private final int maxHeight;
|
||||
private final boolean showGroupStatus;
|
||||
|
||||
public CommandSchedule(int maxWidth, int maxHeight, boolean showGroupStatus) {
|
||||
this.maxWidth = maxWidth;
|
||||
this.maxHeight = maxHeight;
|
||||
this.showGroupStatus = showGroupStatus;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
try {
|
||||
Field scheduledCommandsField = CommandScheduler.class.getDeclaredField("m_scheduledCommands");
|
||||
Field sequentialCommandGroupCommandsField = SequentialCommandGroup.class.getDeclaredField("m_commands");
|
||||
Field sequentialCommandGroupCurrentCommandIndexField = SequentialCommandGroup.class.getDeclaredField("m_currentCommandIndex");
|
||||
Field parallelCommandGroupCommandsField = ParallelCommandGroup.class.getDeclaredField("m_commands");
|
||||
Field parallelDeadlineGroupCommandsField = ParallelDeadlineGroup.class.getDeclaredField("m_commands");
|
||||
Field parallelRaceGroupCommandsField = ParallelRaceGroup.class.getDeclaredField("m_commands");
|
||||
AccessibleObject.setAccessible(new Field[] { scheduledCommandsField, sequentialCommandGroupCommandsField, sequentialCommandGroupCurrentCommandIndexField, parallelCommandGroupCommandsField, parallelDeadlineGroupCommandsField, parallelRaceGroupCommandsField }, true);
|
||||
Lookup lookup = MethodHandles.lookup();
|
||||
sequentialCommandGroupCommandsMethod = lookup.unreflectGetter(sequentialCommandGroupCommandsField);
|
||||
sequentialCommandGroupCurrentCommandIndexMethod = lookup.unreflectGetter(sequentialCommandGroupCurrentCommandIndexField);
|
||||
parallelCommandGroupCommandsMethod = lookup.unreflectGetter(parallelCommandGroupCommandsField);
|
||||
parallelDeadlineGroupCommandsMethod = lookup.unreflectGetter(parallelDeadlineGroupCommandsField);
|
||||
parallelRaceGroupCommandsMethod = lookup.unreflectGetter(parallelRaceGroupCommandsField);
|
||||
scheduledCommands = ((LinkedHashMap<Command, Object>) lookup.unreflectGetter(scheduledCommandsField).invoke(CommandScheduler.getInstance()));
|
||||
} catch (Throwable e) {
|
||||
LOGGER.log(Level.SEVERE, "Failed to reflect necessary fields to run the command schedule.", e);
|
||||
cancel();
|
||||
return;
|
||||
}
|
||||
root = Shuffleboard.getTab("Command Schedule").getLayout("Command Schedule", BuiltInLayouts.kGrid).withSize(maxWidth, maxHeight);
|
||||
Shuffleboard.selectTab("Command Schedule");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
int size = scheduledCommands.size();
|
||||
root.withProperties(Map.of("Number of columns", size, "Number of rows", 1, "Label position", "TOP"));
|
||||
for (Command command : scheduledCommands.keySet()) {
|
||||
putCommand(command, root, size, command::isScheduled);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
sequentialCommandGroupCommandsMethod = null;
|
||||
sequentialCommandGroupCurrentCommandIndexMethod = null;
|
||||
parallelCommandGroupCommandsMethod = null;
|
||||
parallelDeadlineGroupCommandsMethod = null;
|
||||
parallelRaceGroupCommandsMethod = null;
|
||||
root = null;
|
||||
ungroupedLayout = null;
|
||||
scheduledCommands = null;
|
||||
purgeShuffleboardTab("Command Schedule");
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return isScheduled() ? "Enabled" : "Disabled";
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean runsWhenDisabled() {
|
||||
return true;
|
||||
}
|
||||
|
||||
private void putCommand(Command command, ShuffleboardContainer layout, int siblings, BooleanSupplier running) {
|
||||
boolean isRoot = root == layout;
|
||||
String name = command.getName() + "@" + Integer.toHexString(command.hashCode());
|
||||
if (command instanceof CommandGroupBase) {
|
||||
Collection<Command> commands = List.of();
|
||||
Function<Command, BooleanSupplier> nestedRunningMaker = c -> () -> !c.isFinished();
|
||||
if (command instanceof SequentialCommandGroup) {
|
||||
ArrayList<Command> commandsList = Errors.log().getWithDefault(() -> (ArrayList<Command>) sequentialCommandGroupCommandsMethod.invoke(command), new ArrayList<>());
|
||||
commands = commandsList;
|
||||
nestedRunningMaker = c -> () -> Errors.log().getWithDefault(() -> (int) sequentialCommandGroupCurrentCommandIndexMethod.invoke(command) == commandsList.indexOf(c), false);
|
||||
} else if (command instanceof ParallelCommandGroup) {
|
||||
HashMap<Command, Boolean> commandsMap = Errors.log().getWithDefault(() -> (HashMap<Command, Boolean>) parallelCommandGroupCommandsMethod.invoke(command), new HashMap<Command, Boolean>());
|
||||
commands = commandsMap.keySet();
|
||||
nestedRunningMaker = c -> () -> commandsMap.get(c);
|
||||
} else if (command instanceof ParallelDeadlineGroup) {
|
||||
commands = Errors.log().getWithDefault(() -> (HashMap<Command, Boolean>) parallelDeadlineGroupCommandsMethod.invoke(command), new HashMap<Command, Boolean>()).keySet();
|
||||
nestedRunningMaker = c -> () -> !command.isFinished();
|
||||
} else if (command instanceof ParallelRaceGroup) {
|
||||
commands = Errors.log().getWithDefault(() -> (HashSet<Command>) parallelRaceGroupCommandsMethod.invoke(command), new HashSet<>());
|
||||
nestedRunningMaker = c -> () -> !command.isFinished();
|
||||
}
|
||||
ShuffleboardLayout nestedLayout;
|
||||
int size = commands.size() + (showGroupStatus ? 1 : 0);
|
||||
if (isRoot)
|
||||
nestedLayout = layout.getLayout(name, BuiltInLayouts.kList).withSize(maxWidth / siblings, maxHeight);
|
||||
else
|
||||
nestedLayout = layout.getLayout(name, BuiltInLayouts.kGrid).withProperties(Map.of("Number of columns", size, "Number of rows", 1));
|
||||
if (showGroupStatus && nestedLayout.getComponents().stream().map(ShuffleboardComponent::getTitle).noneMatch("_self_"::equals))
|
||||
nestedLayout.addBoolean("_self_", running);
|
||||
for (Command nestedCommand : commands) {
|
||||
putCommand(nestedCommand, nestedLayout, size, nestedRunningMaker.apply(nestedCommand));
|
||||
}
|
||||
} else if (command instanceof CommandBase) {
|
||||
ShuffleboardContainer target = isRoot ? Objects.requireNonNullElseGet(ungroupedLayout, () -> ungroupedLayout = root.getLayout("Ungrouped", BuiltInLayouts.kList)) : layout;
|
||||
if (target.getComponents().stream().map(ShuffleboardComponent::getTitle).noneMatch(name::equals))
|
||||
target.addBoolean(name, running);
|
||||
}
|
||||
}
|
||||
|
||||
private static void purgeShuffleboardTab(String name) {
|
||||
Shuffleboard.getTab(name).getComponents().clear();
|
||||
NetworkTable rootTable = NetworkTableInstance.getDefault().getTable("Shuffleboard");
|
||||
NetworkTable rootMetaTable = rootTable.getSubTable(".metadata");
|
||||
recursiveClearTable(rootMetaTable.getSubTable(name));
|
||||
recursiveClearTable(rootTable.getSubTable(name));
|
||||
rootMetaTable.getEntry("Selected").setString("");
|
||||
rootMetaTable.delete(name);
|
||||
rootTable.delete(name);
|
||||
try {
|
||||
Field shuffleboardRootField = Shuffleboard.class.getDeclaredField("root");
|
||||
shuffleboardRootField.trySetAccessible();
|
||||
Object shuffleboardRoot = shuffleboardRootField.get(null);
|
||||
Field shuffleboardTabsField = shuffleboardRoot.getClass().getDeclaredField("m_tabs");
|
||||
Field shuffleboardTabsChangedField = shuffleboardRoot.getClass().getDeclaredField("m_tabsChanged");
|
||||
shuffleboardTabsField.trySetAccessible();
|
||||
shuffleboardTabsChangedField.trySetAccessible();
|
||||
((LinkedHashMap<String, ShuffleboardTab>) shuffleboardTabsField.get(shuffleboardRoot)).remove(name);
|
||||
shuffleboardTabsChangedField.set(shuffleboardRoot, true);
|
||||
} catch (NoSuchFieldException | IllegalAccessException e) {
|
||||
LOGGER.log(Level.SEVERE, "Failed to purge Shuffleboard tab " + name + ".", e);
|
||||
}
|
||||
Shuffleboard.update();
|
||||
}
|
||||
|
||||
private static void recursiveClearTable(NetworkTable table) {
|
||||
table.getSubTables().forEach(name -> recursiveClearTable(table.getSubTable(name)));
|
||||
table.getSubTables().forEach(table::delete);
|
||||
table.getKeys().forEach(table::delete);
|
||||
}
|
||||
}
|
||||
@@ -51,7 +51,6 @@ public class VisionOdometry extends SubsystemBase {
|
||||
|
||||
setLEDs(false);
|
||||
setDriverMode(false);
|
||||
PhotonCamera.setVersionCheckEnabled(false);
|
||||
}
|
||||
|
||||
/** Gets the vision points from the limelight
|
||||
|
||||
@@ -1,130 +0,0 @@
|
||||
package frc4388.utility;
|
||||
|
||||
import java.lang.invoke.MethodHandle;
|
||||
import java.lang.invoke.MethodHandles;
|
||||
import java.lang.reflect.AccessibleObject;
|
||||
import java.lang.reflect.Field;
|
||||
import java.lang.reflect.InvocationTargetException;
|
||||
import java.lang.reflect.Method;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Collection;
|
||||
import java.util.HashMap;
|
||||
import java.util.HashSet;
|
||||
import java.util.LinkedHashMap;
|
||||
import java.util.List;
|
||||
import java.util.Map;
|
||||
import java.util.Objects;
|
||||
import java.util.Set;
|
||||
import java.util.Map.Entry;
|
||||
import java.util.function.BooleanSupplier;
|
||||
import java.util.function.Function;
|
||||
import java.util.function.Predicate;
|
||||
import java.util.function.Supplier;
|
||||
import java.util.stream.Collectors;
|
||||
|
||||
import com.diffplug.common.base.Errors;
|
||||
|
||||
import edu.wpi.first.math.Pair;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.LayoutType;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardContainer;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import edu.wpi.first.wpilibj2.command.CommandGroupBase;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
|
||||
public final class Commander {
|
||||
private static MethodHandle m_scheduledCommandsHandle;
|
||||
private static MethodHandle m_sequentialCommandGroupCommandsHandle;
|
||||
private static MethodHandle m_sequentialCommandGroupCurrentCommandIndexHandle;
|
||||
private static MethodHandle m_parallelCommandGroupCommandsHandle;
|
||||
private static MethodHandle m_parallelDeadlineGroupCommandsHandle;
|
||||
private static MethodHandle m_parallelRaceGroupCommandsHandle;
|
||||
private static ShuffleboardTab tab;
|
||||
|
||||
public static void initialize() {
|
||||
try {
|
||||
Field m_scheduledCommandsField = CommandScheduler.class.getDeclaredField("m_scheduledCommands");
|
||||
m_scheduledCommandsField.trySetAccessible();
|
||||
m_scheduledCommandsHandle = MethodHandles.lookup().unreflectGetter(m_scheduledCommandsField);
|
||||
Field m_sequentialCommandGroupCommandsField = SequentialCommandGroup.class.getDeclaredField("m_commands");
|
||||
m_sequentialCommandGroupCommandsField.trySetAccessible();
|
||||
m_sequentialCommandGroupCommandsHandle = MethodHandles.lookup().unreflectGetter(m_sequentialCommandGroupCommandsField);
|
||||
Field m_sequentialCommandGroupCurrentCommandIndexField = SequentialCommandGroup.class.getDeclaredField("m_currentCommandIndex");
|
||||
m_sequentialCommandGroupCurrentCommandIndexField.trySetAccessible();
|
||||
m_sequentialCommandGroupCurrentCommandIndexHandle = MethodHandles.lookup().unreflectGetter(m_sequentialCommandGroupCurrentCommandIndexField);
|
||||
Field m_parallelCommandGroupCommandsField = ParallelCommandGroup.class.getDeclaredField("m_commands");
|
||||
m_parallelCommandGroupCommandsField.trySetAccessible();
|
||||
m_parallelCommandGroupCommandsHandle = MethodHandles.lookup().unreflectGetter(m_parallelCommandGroupCommandsField);
|
||||
Field m_parallelDeadlineGroupCommandsField = ParallelDeadlineGroup.class.getDeclaredField("m_commands");
|
||||
m_parallelDeadlineGroupCommandsField.trySetAccessible();
|
||||
m_parallelDeadlineGroupCommandsHandle = MethodHandles.lookup().unreflectGetter(m_parallelDeadlineGroupCommandsField);
|
||||
Field m_parallelRaceGroupCommandsField = ParallelRaceGroup.class.getDeclaredField("m_commands");
|
||||
m_parallelRaceGroupCommandsField.trySetAccessible();
|
||||
m_parallelRaceGroupCommandsHandle = MethodHandles.lookup().unreflectGetter(m_parallelRaceGroupCommandsField);
|
||||
} catch (IllegalArgumentException | NoSuchFieldException | SecurityException | IllegalAccessException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
tab = Shuffleboard.getTab("Commander");
|
||||
try {
|
||||
scheduledCommands = ((LinkedHashMap<Command, Object>) m_scheduledCommandsHandle.invoke(CommandScheduler.getInstance()));
|
||||
} catch (Throwable e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
private static LinkedHashMap<Command, Object> scheduledCommands;
|
||||
|
||||
public static void periodic() {
|
||||
int count = scheduledCommands.size();
|
||||
for (Command command : scheduledCommands.keySet()) {
|
||||
putCommand(command, null, count, command::isScheduled);
|
||||
}
|
||||
}
|
||||
private static int count = 0;
|
||||
private static void putCommand(Command command, ShuffleboardContainer layout, int siblings, BooleanSupplier running) {
|
||||
String name = (count++) + ":" + command.getClass().getSimpleName()/* + "@" + Integer.toHexString(command.hashCode()) */;
|
||||
if (Objects.requireNonNullElse(layout, tab).getComponents().stream().map(ShuffleboardComponent::getTitle).anyMatch(name::equals)) return;
|
||||
if (command instanceof CommandGroupBase) {
|
||||
Collection<Command> commands = List.of();
|
||||
String glyph = "CUBE";
|
||||
Function<Command, BooleanSupplier> nestedRunningMaker = c -> () -> !c.isFinished();
|
||||
if (command instanceof SequentialCommandGroup) {
|
||||
ArrayList<Command> commandsList = Errors.log().getWithDefault(() -> (ArrayList<Command>) m_sequentialCommandGroupCommandsHandle.invoke(command), new ArrayList<>());
|
||||
commands = commandsList;
|
||||
nestedRunningMaker = c -> () -> Errors.log().getWithDefault(() -> (int) m_sequentialCommandGroupCurrentCommandIndexHandle.invoke(command) == commandsList.indexOf(c), false);
|
||||
glyph = "LIST_OL";
|
||||
} else if (command instanceof ParallelCommandGroup) {
|
||||
HashMap<Command, Boolean> commandsMap = Errors.log().getWithDefault(() -> (HashMap<Command, Boolean>) m_parallelCommandGroupCommandsHandle.invoke(command), new HashMap<Command, Boolean>());
|
||||
commands = commandsMap.keySet();
|
||||
nestedRunningMaker = c -> () -> commandsMap.get(c);
|
||||
glyph = "LIST_UL";
|
||||
} else if (command instanceof ParallelDeadlineGroup) {
|
||||
HashMap<Command, Boolean> commandsMap = Errors.log().getWithDefault(() -> (HashMap<Command, Boolean>) m_parallelDeadlineGroupCommandsHandle.invoke(command), new HashMap<Command, Boolean>());
|
||||
commands = commandsMap.keySet();
|
||||
nestedRunningMaker = c -> () -> commandsMap.get(c);
|
||||
glyph = "CROSSHAIRS";
|
||||
} else if (command instanceof ParallelRaceGroup) {
|
||||
commands = Errors.log().getWithDefault(() -> (HashSet<Command>) m_parallelRaceGroupCommandsHandle.invoke(command), new HashSet<>());
|
||||
nestedRunningMaker = c -> () -> !command.isFinished();
|
||||
glyph = "RANDOM";
|
||||
}
|
||||
ShuffleboardLayout nestedLayout = Objects.requireNonNullElse(layout, tab).getLayout(name, layout == null ? BuiltInLayouts.kList : BuiltInLayouts.kGrid).withSize(11 / siblings, layout == null ? 6 : 1).withProperties(Map.of("Number of columns", 11 / siblings, "Number of rows", 1, "Label position", "BOTTOM", "Show Glyph", true, "Glyph", glyph));
|
||||
int count = commands.size();
|
||||
for (Command nestedCommand : commands) {
|
||||
putCommand(nestedCommand, nestedLayout, count, nestedRunningMaker.apply(nestedCommand));
|
||||
}
|
||||
} else if (command instanceof CommandBase) {
|
||||
Objects.requireNonNullElse(layout, tab).addBoolean(name, running);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user