diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 15ee89d..7bc646f 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -18,6 +18,7 @@ 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; @@ -57,7 +58,7 @@ 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()); @@ -93,7 +94,7 @@ 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); @@ -137,17 +138,6 @@ public class Robot extends TimedRobot { public void disabledInit() { LOGGER.fine("disabledInit()"); m_robotTime.endMatchTime(); - if (isTest()) { - // IMPORTANT: Had to chown the pathplanner folder in order to save autos. - File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner") - .resolve("recording." + System.currentTimeMillis() + ".path").toFile(); - if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) { - // m_robotContainer.createPath(null, null, false).write(outputFile); - LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath()); - } else - LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); - } - m_robotContainer.m_robotVisionOdometry.setLEDs(false); } @@ -179,7 +169,9 @@ public class Robot extends TimedRobot { * This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + final int a = 1; + } @Override public void teleopInit() { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6495570..c14d389 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -122,6 +122,9 @@ public class RobotContainer { private SendableChooser 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 @@ -625,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), - buildAuto(3.0, 3.0, "JMove")); + autoJMove1); 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), - buildAuto(3.0, 3.0, "JMove2")); + autoJMove2); 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); diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index fda2586..861ded3 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -51,6 +51,7 @@ public class VisionOdometry extends SubsystemBase { setLEDs(false); setDriverMode(false); + PhotonCamera.setVersionCheckEnabled(false); } /** Gets the vision points from the limelight diff --git a/src/main/java/frc4388/utility/Commander.java b/src/main/java/frc4388/utility/Commander.java new file mode 100644 index 0000000..1e37e24 --- /dev/null +++ b/src/main/java/frc4388/utility/Commander.java @@ -0,0 +1,130 @@ +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) m_scheduledCommandsHandle.invoke(CommandScheduler.getInstance())); + } catch (Throwable e) { + e.printStackTrace(); + } + } + private static LinkedHashMap 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 commands = List.of(); + String glyph = "CUBE"; + Function nestedRunningMaker = c -> () -> !c.isFinished(); + if (command instanceof SequentialCommandGroup) { + ArrayList commandsList = Errors.log().getWithDefault(() -> (ArrayList) 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 commandsMap = Errors.log().getWithDefault(() -> (HashMap) m_parallelCommandGroupCommandsHandle.invoke(command), new HashMap()); + commands = commandsMap.keySet(); + nestedRunningMaker = c -> () -> commandsMap.get(c); + glyph = "LIST_UL"; + } else if (command instanceof ParallelDeadlineGroup) { + HashMap commandsMap = Errors.log().getWithDefault(() -> (HashMap) m_parallelDeadlineGroupCommandsHandle.invoke(command), new HashMap()); + commands = commandsMap.keySet(); + nestedRunningMaker = c -> () -> commandsMap.get(c); + glyph = "CROSSHAIRS"; + } else if (command instanceof ParallelRaceGroup) { + commands = Errors.log().getWithDefault(() -> (HashSet) 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); + } + } +}